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ABSTKACr 






I he performance of a Kalman (iltcr used to track a hurricane was substantially im- 
proved by implementing a fixed interval smoothing algorithm. 1 his tracking routine was 
designed and inijilcnicntcd in a microcomputer program. Several tracking sccnaiios were 
simulated and analyzed. Actual storm tracks obtained from the Joint Typhoon Warning 
Center in Guam, Mariana Islands, were used for this research. 1 he application of the 
Kalman tracker to a tropical storm s wind speed tracking was also investigated by using 
the best track data and observed data. 



/VI93^^3 



THESIS DISCLAIMER 



Ihc reader is cautioned that computer programs developed in this research may not 
have been exercised for all eases ol inleresl. While every ellort has been .aade, vvuhm 
Ihc time available, lo ensure lhal the programs arc lice of computational and logic er- 
rors, they cannot be considered validated. Any application of these programs rvithonl 
additional verification is at the risk of the user. 
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I. INTRODUCTION 



Conceived over warm tropical oceans, born amid torrential thundershowers, and 
nurtured by water vapor drawn inward from far away, the mature tropical cyclone is an 
ofl'spring of the atmosphere with both negative and positive consequences for life. Se- 
vere cyclones are among the most destructive of all natural disasters, capable of annihi- 
lating coastal towns and killing hundreds of thousands of people. On the positive 
though less dramatic side, they provide essential rainfall over much of lands they cross. 
It is diiricult to convey to those who have never experienced a tropical cyclone the hor- 
ror that great hurricanes can bring to ships at sea or people living near the coast. 
Tropical cyclones cause a variety of damage and the same tropical cyclone often alTccts 
several nations during its lifetime. They are called "Hurricanes" in the Atlantic and 
eastern Pacific" [Ref IJ. Hurricanes were identified by female or male names like I'at 
and Tess. These storms will be discussed in this thesis, 'fropical cyclones are also 
numbered sequentially according to their starting date. This numbering system is used 
with caution when referencing storms from other data bases. 

This thesis attempted to improve the estimation of the hurricane's future course, 
speed, and position by using a Kalman filter with smoothing. 'I'his problem is similar 
to the ship tracking problem which is discussed in a previous thesis IRcf 2). The major 
dilference between ship tracking and storm tracking problem is the measurement process 
which is given actual position coordinates (latitude and longitude) in the storm tracking 
problem, l liercforc, the linearization required in the ship tracking problem is unneces- 
sary in the storm tracking problem. The measurement noise varies with the type of the 
sensor (aircraft, satellite, and radar). 

An accurate and reliable method of tracking and targeting is necessary. 'I'he current 
methods used to track a storm include the use of radar, aircraft, and satellite. However, 
the data may or may not be available when needed for a number of reasons. As an ex- 
ample, aircraft may not be available due to flight restrictions. A Kalman filter with a 
fixed interval smoothing algorithm can be used to track a storm. The smoothing algo- 
rithm is an ofi-line calculation that uses all measurements taken during a time interval 
0< k< M to improve the estimate. By having a more accurate assessment of what the 
storm has done in the past, we will be better able to predict ahead and estimate a storm’s 
future course, speed, and position. 



The csliinaiion of the wind speed is as important as the storm position estimate. In 
an ellbrt to estimate the possible damage a hurricane's sustained winds and storm singe 
could do to a coastal area, the Kalman lilter and the smoother was used to estimate the 
wind speed and to categorize the hurricane. If the wind speed estimate is accurate, a 
hurricane is categorized correctly. This thesis attempts to estimate the hurricane's future 
wind speed. This will help to design a timely warning system. 
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II. PROBLEM STATEMENT 



' A. GENERAL 

The storm-tracking scenario parallels the ship tracking problem in that both prob- 
lems developed a position, course, and speed solution for a target with similar system 
dynamics. The tracking scenario used here involves two storms. The positions of the 
storms arc given in a- (longitude), and y (latitude) coordinates. This problem will be 
analyzed using state space methods. Given the longitude and latitude (the measure- 
ments) received by a radar, aircraft, or satellite, we arc interested in estimating the lo- 
cation, course, and speed of the storm (the states of the plant). I hc state variables for 
this plant arc .v, T, y, and y. 

B. SYS! EM MODEL 

This system can be described by the state space equation 

Xk+] = ^>t^k + ^'^k (-•!) 



where 

Xf = state vector to be estimated, 

(/)*= state transition matrix which describes how the states of the dynamic system arc 
related, and 

M'^= random forcing function with a covariance matrix 2* ^^hat is defined as 



Qk - 



100 0 0 0 
0 100 0 0 
0 0 100 0 
000 100 



The state vector is 



( 2 . 2 ) 







( 2 . 3 ) 



and the system state equations are 
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C. MEASUREMENT MODEL 

The measurements are linearly related to the state variables, using the measurement 
equation 



Zfe = + 1'* 



(2.5) 



Since the x and y position states are observed directly and given by latitude and longi- 
tude position coordinates, the measurement equation can be written as 
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( 2 . 6 ) 



where the measurement noise y* has a variance associated with the source of the meas- 
urement. In this thesis, mean deviation (nin) of satellite-derived tropical cyclone posi- 
tions from best track positions (PCN values) were used in the calculation of the 
measurement noise covariance matrix for the satellite data. The measurement noise 
covariance matrix values and PCN values are shown in Table 1. The equation used in 
this calculation is 



Rj^ = {Mean deviation) 



2 



( 2 . 7 ) 



Table 1. THE MEASUREMENT NOISE COVARIANCE MATRIX VALUES 
FOR SATELLITE , 



PCX 


Mean Deviation 




1. or 2 


16 


256 


3. or 4 


3U 


9(i() 


5. or 6 


40 


1600 



'I he measurement noise covariance matrix values were calculated by using the ac- 
curacy number for the aircraft and radar data. Equation (2.8) was used for aircraft data 
and Equation (2.9) was used for the radar data 

» 
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/ *■ 2 2 
~ \J + {Meteorological) ) 



( 2 . 8 ) 



2 

R)( = {Radar Accuracy) (2.9) 

where the radar accuracy numbers arc shown in Table 2 



Table 2. THE MEASUREMENT NOISE COVARIANCE MATRIX VALUES 
FOR RADAR 



Accuracy Number 


Radar Accuracy 


R. 


1, 4, or G 


10 


100 


2, 5. or F 


15 


225 


3, 6, or P 


25 


625 


7, or Blank 


30 


900 



D. KALMAN FILTER 

Basically, the Kalman filter takes an a priori estimate of the states, projcet.s it ahead 
in time to some predicted estimate, and then calculates a gain vector based on the error 
covariance of these estimates. 'I he error between the observed incasurcnicnts and the 
predicted measurements of the corresponding state estimates is multiplied by the gtiin 
vector and the result is added to the predicted state estimates to give the best estimate 
of the true states based on optimal combinations of a priori estimates and cuirenl 
measurements. 

The Kalman filter is the proper algorithm to be used when both the system model 



and the measurement model arc linear functions of the stale variables. 1 he basic oper- 
ation of the filler is a relatively straightforward recursive process. I hc equations used 
in the Kalman filter [Ref 3] arc 


\ 

■ik+\ = + r k^i 


(2.10) 


2-k = ^h-^k + 


(2.11) 


A J ^ 

=i’(A1A--l) = Vk-^{k\k) 


(2.12) 
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^\k\k-\) — ^’k^\k\k)'Pk + Qk 


(2.13) 






(.M4) 



(2.15) 







(2.16) 



where 

i<»!»-i) ~ projected ahead state estimate, 

= projected aliead state error covariance matrix, 

G* = Kalman gain matrix, 

/?* = state measurement noise covariance matrix, and 
//* = linearized measurement matrix. 

The Kalman gain matrix serves to minimize the mean square estimation error and 
is an indication of how much weight will be placed on the current observation. A large 
gain, indicating a large error covarince, will place more weight on the current observa- 
tion as the filter tries to correct the states. The gain matrix is proportional to the vari- 
ance of the uncertainty in the estimate and inversely proportional to the variance of the 
measurement noise. It can be expressed as 



An initial velocity estimate is taken to be zero since there is no velocity information 
at the beginning. 'I'he initial state estimates carr>' with them some error and it is this 
error, or rather an estimate of this error, that is used to construct the initial error 
covariance matrix. The initial position error was estimated to be 10 nautical miles in the 
.ry direction and the initial velocity was estimated to be 0. 15S nautical miles per minute. 
The error was assumed to be zero mean and uncorrelated, ^^'ith these approximations, 
the initial error covariance matrix is given by 



0 0 0 0.025 

E. SMOOTHING ALGORITHM 

Smoothing is a procedure that uses all of the state estimates produced by an esti- 
mator and attempts to improve the accuracy of these estimates by using the negative 




(2.17) 



A0|-1) - Q 



100 0 0 0 

0 0.025 0 0 

0 0 100 0 



(2.18) 
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time dynamics to produce the smoothed estimate. The estimator used here is the 
Kalman filter. The basic idea behind smoothing is that, for a time interval from 0 to K 
{K > k), an estimate at time k based on all previous estimates up to time K, ( ), 

^ will be more accurate than an estimate based only on the estimates up to time k, (.r,, ,,). 
" It is a non-real time operation where the available data are processed to obtain an es- 
timate for some past value ofk " [Ref. 4]. 

Smothing algoritlims were categorized into three groups by Meditch [Ref. 5]; 

Fixed Point Smoothing smooths the estimate x^^,^ at a fi.xcd point k as K increases. 

Fixed Lag Smoothing smooths the estimate x{K— A'| K) at a fi.xed delay N as K in- 
creases. 

Fixed Interval Smoothing smooths the estimate x^^.^ over the time interval from 0 to 
K where K is fixed and k varies from 0 to K. 

A fixed-interval smoothing algorithm was used in this thesis. This smoothing rou- 
tine provides the optimal state estimate at each time k over a fixed interval from 0 to 
K. The equations used in the smoothing algorithm [Ref 5] are 



''h — 


(2.19) 


■^(k\^■) = ^{k\k) + 1 1 /<)) 


(2.20) 


^\k\\') — ^{k\k) + "^/c(^(fc+l|/V) “ ^\k+]\k))'‘h 


(2.21) 



where 

At = smoothing filter gain matrix, 

= smoothed state estimate a time k based on N observations, and 
= smoothed state error covariance matrix. 

At the beginning of the smoothing, the last filtered estimate becomes the initial 
smoothed estimate. The index k is decremented by one for each pass during the 
smoothing algorithm with the starting value of/c equal to the number of data points to 
be smoothed, minus one (A' — 1 ). Consequently, the tracking program makes (A‘— I) 
passes through the smoothing algorithm. 
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in. STORM TRACKING 



' A. GENERAL 

The Kalman filter program STORM. FOR was used in computer simulations. This 

program was originally written for a ship tracking problem and was modified to use on 

storm tracking problem. The graphing routines of the MATLAll were used to generate 

the graphs. A complete listing of the program is included in Appendix A. Typhoon Tess 

and Typhoon Pat were used for simulations. The storm tracks used were obtained from 

data collected at the Joint Typhoon Warning Center located in Guam. Each storm is 

given a separate deck name. Tropical cyclones are numbered sequentially according to 

their starting date by the J1 WC. There are four types of data: 

Best Track -This file is the 6-hourly storm positions based on a post storm, 
subjectively smoothed path. 

forecasts -This data contains the real time storm positions, objective forecasts, and the 
official forecast. Each date-time group may contain one, two, or all three types of 
data. 

Forecast Errors -Eight dincrent errors were computed for each of the objective and 
official forecasts. 

Fixes -Tropical cyclone fixes (observations) from four dificrent platforms arc con- 
tained in the data base. 

The position coordinates were obtained using aircraft, satellite, and radar. The data 
obtained included: raw data (observations); best track data; and 12, 24, and 48 hour 
predictions. The raw data was processed just as if it was real-time observation of the 
hurricane. The first storm, Pat, originated east of Taiwan in the western Pacific on 24 
August 1985. llie warning period for this storm was six days. The storm traveled 
nm. The maximum speed of the wind was over 107 kt and the minimum sea level pres- 
sure was 1002 mb. The Typhoon Pat caused significant damage in southwestern and 
northeastern Japan; primarly on the islands of Kyushu and Hokkaido. Kyushu was hit 
the hardest with wind gusts of 107 kt. A total of 23 people were reported killed with over 
180 people injured. An estimated 3000 homes were damaged. Pat also severely dis- 
rupted transportation by land, sea, and air. 

The second storm track analyzed was that of Typhoon Tess which originated 
southeast of Guam on 30 August 1985. 'fhe warning period for this storm was seven 
days. The storm traveled 1470 nm with maximum wind speeds of over 90 kt. The storm 
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brought needed rain to tlie Philippines during a spell of drier than normal weather. Tlic 
storm also brouglu death and destruction. Considerable flooding and crop damage oc- 
curred over southern China as Tess moved inland [Ref. 6). The observed track of 
Typhoon Pat and Typhoon l ess are shown in Figure 1 and Figure 2, respectively. 
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Figure 1. The observed (rack of Typhoon Pat [Ref. 6 ] 
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Figure 2, The observed track of Typhoon Tess (Ref. 6 ] 
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B. COMPUTER SIMULATIONS 



1. Typhoon Pat 

The best track of Typhoon Pat is shown in Figure 3. The best track positions 
, arc in 6-hourly increments. The first tracking data point corresponds to the day-time 
group 08270000Z. Figure 4 shows the Kalman filter position estimates and Figure 5 
shows the smoothed position estimates. Figure 6 was constructed using the filtered and 
smoothed position estimates. In general, the smoother does improve the track accuracy. 
In the area of the track where the true positions do vary, the smoother tracking error is 
zero. Specifically, this area occurs between 23' N, 124° E, and 38° N, 133° E. This area 
can be seen easily in Figure 7. This figure was constructed by using the tracking error 
of the filter and smoother. The average tracking errors for this storm arc ± 4 nautical 
miles for the filter and + 2 nautical miles for the smoother estimates. 

2 . Typhoon Tess 

The performance of the smoother on the track of Typhoon Tess was similar to 
that of Typhoon Pat. Figure 8 shows Typhoon Tess best track. Typhoon Tess best 
track data are also in 6-hourly increments, fhe filter and smoother tracking results arc 
shown in Figure 9 and Figure 10, respectively. Figure 11 shows the track results ob- 
tained with the Kalman filter and smoothing algorithm, 'fhe smoother shows some im- 
provement near 17.5° N, 120°E and 15.2° N, 130°E. The filter average tracking error 
increased slightly, to about + 5 nm, but the smoother average tracking error jump to 
about ± 5 nm. 'Ihis is because the smoother gives 30 nautical miles tracking error near 
18.8° N, 116°E due to large change on the direction of Typhoon 'fess. Figure 12 shows 
the tracking errors of the filter and smoother. It is observed that the smoother was 
much less sensitive to the large course changes than the Kalman filter. It is, therefore, 
reasonable to assume that similar results could be expected from the smoother lor a 
large course change more than 90° . However, the smoother's estimates arc quite good 
over the entire trajectory and the estimates closely follow* the course changes as in 
Typhoon Pat. 
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rigm e 3. The Dest Track of Typhoon Pat 
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figure 4 . Filleicd Trnck of Typhoon Pnt 
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Figme 5. Smoothed Track of Typlioon Pat 
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Figure 6. riKercd and Sinoodied Track of Typhoon Pat 
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Figure 7. Tracking Errors of ’ the Filler and Smoollier lor typlioon Pat 
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Figure 8. Tlie Best Trnck of Typhoon Tess 
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Figure 9. Filleictl Track of Typhoon Tess 
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rigtire 10. Sinoodicd Track of Tj'plioon Tess 
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Figure 11. Fillci ed and Siiiootlicd Track of Typhoon Tess 
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IV. STORM WIND TRACKING 



' A. GENERAL 

In an clfort to estimate the possible damage a hurricane's sustained winds and 
storm surge could do to a coastal area, the SalTir-Simpson damage-potential scale was 
developed. The scale numbers are based on actual conditions at some time during the 
life of the storm " [Ref 7J. Table 3 shows these categories. 



Table 3. SAFFIR-SIMESON HURRICANE DAMAGE-POTENTIAL SCALE 



Scale Num- 
ber 


Wind 

speed(knots) 


Damage 


I 


64-82 


Damage mainly to trees, shrubbery, and unanchored 
mobile homes. 


2 


83-95 


Some trees blown down; major damage to exposed 
mobile homes; some damage to roofs of buildings. 


3 


96-113 


Foliage removed from trees; large trees blown down; 
mobile homes destroyed; some structural damage to 
small buildings. 


4 


114-135 


All signs blown down; extensive damage to roofs, win- 
dows, and doors; complete destruction of mobile 
homes; flooding inland as far. 


5 


> 135 


Severe damage to windows and doors; extensive dam- 
age to roofs of homes and industrial buildings; small 
buildings overturned and blown away; major damage 
to lower floors of all structures less than 4.5 m above 
sea level within 500 m of shore. 



1 he storm wind tracking scenario parallels the storm tracking problem. The track- 
ing scenario used here involves two storms. This problem (vill be anah'zcd using state 
space methods. Given the tropical cyclone intensity values the observed speed of the 
storm wind will be estimated by using the Kalman filter and smoother, fable 4 shows 
the relationship between intensity and wind speed. The wind speed was used directly as 
a measurement for the best track data of the storm. The state variables for this plant 
are iv, and w. 

The system can be described by the state space equation 

l£k+\ == ^'k'-^k+fk H'l) 



2.3 



where 



w*= state vector to be estimated, 

(/)*= state transition matrix which describes how the states of tlie dynamic system are 
related, and 

yi= random forcing function with a covariance matrix <2* tliat is defined as 



Qk — 








(4.2) 



The state vector is 



H'* = [*] (‘'•3) 

and the system state equations are 

H.4) 

The measurements are linearly related to the state variables. Using the measurement 
equation 



Zk = U 



(4.5) 



'fhe measurement equation can be written as 

z;,+i = [l • (4.6) 

where the measurement noise v* has a variance associated with the source of the meas- 
urement. 1 he measurement noise covariance matrix values are calculated in the same 
manner as in storm position tracking problem by using Equations (2.8) and (2.9) for the 
aircrait and radar data. 

fhe initial error covariance matrix used in the wind speed tracking is 
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1000000 000 
0 0.25 0 0 

0 0 1000000 0 

0 0 0 0.2.5 



Table 4. MAXIMUM SUSTAINED WIND SPEED AS A I'UNCllON UP 
FORECAST INTENSITY NUMBER 



Intensity 


Wind speed(nm'h) 


00 


25 


. 05 


25 


10 


25 


15 


25 


20 


30 


25 


35 


30 


45 


35 


55 


40 


65 


45 


77 


50 


90 


55 


102 


00 


1 15 


65 


127 


70 


140 


75 


155 


80 


170 



D. COMPUTER SIMULATIONS ' 

1. The Best Track Data 
a. Typhoon Pat 

Using the best track data wind speed values as the measurements, future 
wind speed values were estimated by the filter and the smoother. There is an initial track 
error due to the error in the initial state estimates. When the wind speed increases at 
24 hours, the tracking error decreases and becomes zero for the fifth data as the filter 
gains the wind track. However, it increases after 00 hours when the wind speed de- 
creases very fast and it returns to zero two data points later as the filter regains the wind 



25 



track. Figure N shows the filter tracking accuracy. The smoother is not as accurate as 
in the position estimate due to the large change in wind speed, but tlicsc errors remain 
in the acceptable ranges. The smoother track is shown in Figure 15. The average 
, tracking errors arc +0.5 niph for the filter and ±1.1 mph for the smoother, best track 
data represents the weather service's estimate of truth [Ref 6]. Figure 16 compares the 
forward time estimate (filter, FlL(o)) with the forward and negative time estimate 
(smoother, SM(x)) for Typhoon Pat. Figure 17 denotes the error in these estimates. 
b. Typhoon Tess 

The tracking results for this storm are shown in Figures 18-22. From Figure 
19 and 20, we can see how the Kalman filter and the fixed interval smoothing improve 
the overall track estimate. During the overall track estimate, two large filter tracking 
errors arc detected. This is shown in Figure 19. In both instances the smoother also 
gives large tracking errors. Figure 20 shows the smoother estimates. At other times, 
however, the filtered and smoothed estimate arc accurate. Figure 21 is the comparison 
of tlic filter and smoother estimates. The filter average tracking error is ± 1.5 mpli and 
the smoother average tracking error is ± 2.0 mph. Figure 22 shows the tracking errors 
of the filter and smoother estimates. 



I 



26 







(nnOH ^Hd SaTM TVDLLnVH)a33dS aNlA\ 



Figure 13. Tlie Best Track Wind Speed of Typlioon Pat (Ref. 6] 
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STORMl TRACKS - TRU(o) vs FILT(x) 




(HnoH HHd saiM 'TVDLLnvN)a3ads qnlw 



Figure \4. Fillcieil Track of Typhoon Pal's Best Track Wind Speed 



TIME-(HOURS) 




riguic 15. Siiiootiicd Track of Typlioon Pat's Ilcsl Track Wind Speed 
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rigiii c 16. riKci cd and Smoollicd Track of Typhoon Pal's Host Track Wind Speed 
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STORMl WIND TRACKING ERRORS - SM(x) vs FILT(o) 




iinoii yad shtiim ivoiinvN 



Figure 17. The Filter and Sinootlicr 1 racking Errors of Tjphoon Pal 
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DATA POINT 



STORM2 TRACKS-TRUTH 
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Figure 18. Tlie Best 1 rack Wind Speed of Typhoon Tess (Ref. 6] 
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Figure 19. 



Filtered Track of Typhoon Tess' Best Track Wind Speed 
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Figure 21. Filtered and Smoothed Track of Typhoon Tess' Best 'I rack Wind Speed 
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rioiire 22. 1 lie Filler and Smoother Tracking Errors of Typhoon Tess 

• * t' 
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2. The Observed Wind Speed Data 

Ihcrc was uncertainity in the observed data obtained from tlie J’i’WC [Ref. G\. 
Ibis data has more than one data at the same time instant for the dill'erent positions 
irom the eye of the hurricane. This is shown in Figures 23 and 24. There was a strong 
potential for the filter to go unstable. This was data smoothed using the liquations (4.8) 
and (4.9). The data obtained before and after curve fitting is shown in Figures 25 and 
26. 



1.0 


-T-2 


Ttz 


1.0 


- 7--1 


Tl, 


1.0 


0.0 


0.0 


1.0 


7 i 




1.0 




7 ^ 



(4.8) 



h-iiilikY'iihi, 



(4.9) 



where 

measurements to be smoothed, and 
smoothed measurements. 



a. Typhoon Pat 

Using the interpolated data as an observed data, tracking results obtained 
for typhoon Pat are shown in Figures 27 and 28. The filter and smoother estimates the 
wind speed accurately, '1 here is no potential for the filter and smoother to go unstable. 
The accuracy of the filter is about 70°o, and the smoother is about 65"'o. Due to the 
instant change in the wind speed, the smoother cannot adapted to this change easily. 

b. Typhoon Tas 

The performance of the filter and the smoother are better in Typhoon 'l ess. 
They estimate the wind speed veiy accurately. Again, there is no potential for the filter 
and smoother to go unstable. During the tracking scenario the filter gives the actual 
observed value and the smoother does improve the accuracy of these estimates. 'I he 
tracking error is usually zero or very close to zero. The accuracy of the filter and 
smoother are almost the same in this hurricane which is about 85%. The tracking re- 
sults are shown in Figures 29 and 30. 
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figure 23. The Observed Wind Speed at Some Distance of Typlioon fat [Ref. 6 ] 
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ST0RM2 TRACKS - TRU(o) vs OBS(x) 




Figure 2A. The Obscrrcd Wind Speed at Some Distance of I jplioon Toss [Ref. F'J 
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Figure 25. The 01)scr\cil and Intel jiulntcd Track of Typhoon Pat 



STORM2 TRACKS - OBS(o) vs SMO(x) 
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Figure 26. The Observed and IiUerpoIalcd I rack of Typhoon Toss 
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Figure 27. FiKciccl Tr.ick of Fyplioon P.nt's Observed ^^’i^d Speed 
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STORMl TRACKS - OBS(x) vs SM(o) 
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Figure 28. Sniootlied Track of! yi)liooii Pat's Observed Wind Speed 
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Figure 29, Filtered Track of Typhoon Tess' ObseiTed \^■ind Speed 
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Figure 30. Smoothed Track of Typhoon Tess' Observed Wind Speed 



V. CONCLUSIONS 



1 lie purpose of this research was to improve the accuracy and storm tracking ca- 
pability of a Kalman filter tracking by implementing a fixed interval smoothing algo- 
rithm. 'fwo different tropical storms were simulated and the accuracy of the observed, 
the filtered, and the smoothed storm tracks were analyzed and discussed. 

'1 he fixed interval smoothing algoiithm improved the position accuracy of the storm 
in all of the tracking scenarios simulated. 1 lowcvcr, the smoothed result was not alwaj s 
the most accurate for every storm track. The smoother did improve the track accuracy 
on the basis of the best tiack storm positions. The effectiveness of the smoother in- 
creased as the storm lifetiinc incieascd and the storm course change decreased. 

'1 he storm wind speed tracking scheme imj^'lemented worked well. 1 lowc\er. becaii.se 
this tracking involves the addition of a time- varying value of the state excitation matrix. 
(>, , there was a strong potential lor the filter to go unstable. 1 his was observed during 
the storm wind speed tracking. It was difficult to decide the value of 0^ and R^, for ob- 
served wind speed tracking, because intensity could not be observed man}' times. This 
problem was solved by using a curve fitting method and then this data was used for in- 
puts to the tracking problem. '1 he results show that this method can be used to in- 
terpolate the uncertain data and to avoid an unstable filter. 

I he application of the Kalman filter tracker to the storm tracking problem would 
be very useful in attempting to predict the storm's track when little data is available, as 
seen in observed wind speed tracking problem. T hen, by using the filter and smoothing 
algoiithm, track of the storm's past history can be calculated allowing for a more accu- 
rate prediction of the storm's future track. I here was no standard dc\ iation lor obser\ ed 
wind data. If J IAVC can obtain standard deviations for observed wiml data, this can 
be used. '1 he wind data obttiined has much missing data, some times causing an unsta- 
ble filter. 
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APPENDIX A. STORM. FOR 



This is a listing of the STORM. FOR program used to generate the data for the 
target tarcks presented in this thesis. In order to run this program, the STOR.Ml.DAT 
or STORM2.D.AT file must be available. 



C 



STORM 1*** 



1) ENSURE STORM DATA IS AVAILABLE 

2) RUN STORM 1 OR ST0RM2 

3) COPY OESDATA,FILDATA, & SMDATA --> MATLAB SUB-DIR. 

4) BEGIN MATLAB --> RUN STORM 2. M 






THIS PROGRAM EMPLOYS AN ADAPTIVE EXTENDED KALMAN 

FILTER WITH A FIXED INTERVAL SMOOTHING ALGORITHM TO TRACK A 

TROPICAL STORM USING OBSERVED LATITUDES AND LONGITUDES. 



Vf ; V V: Vr tV ‘V 'tc * Vc *.V Vf Vr V? :'r ;V V: tV tV tV : V ‘A* A' A* A- : V Vr A- A' A *A' A' A' *:V A' A' A A A- V A- A- A A- A A- A A A A- A A- '.V A- A A- A- A A A- A- A A- A A 



C —VARIABLE DEFINITIONS-'-'"- 



C 

C 

C 

C 

C 

C 

C 

C 

C 

C 

C 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 



AK 

AKT 

ERG 

BRKKMl 

DBRG 

DT 



DTOR 

E1.E2 

E1M1,E2M1 

E1M2,E2M2 

FACl 

G 

GATEl 



H 

HDG 

HT 

1 

I MAT 



SMOOTHING FILTER GAIN MATRIX 
TRANSPOSE OF AK 

MEASURED TARGET BEARING IN RADIANS 
PREDICTED TARGET BEARING MEASUREMENT 
IN RADIANS BRG(K|K-1) 

MEASURED TARGET BEARING IN DEGREES 
TIME DELAY BETlv’EEN OBSERVATIONS, T(K) 

- T(K1) 

DEGREE TO RADIAN CONVERSION FACTOR 
MEASUREMENT RESIDUAL, Z(K) - K(X(K|K-1)) 
MEASUREMENT RESIDUAL AT PREVIOUS 
OBSERVATION 

MEASUREMENT RESIDUAL T\\’0 OBSERVATIONS 
PREVIOUS 

RECIPROCAL OF VARE 

KALMAN GAIN VECTOR 

1. 5--'^STANDARD DEVIATION OF RESIDUAL 

PROCESS. USED AS A GATE IN 

MA.NEUVER DETECTION 

MEASUREMENT MATRIX 

ESTIMATED TARGET HEADING IN DEGREES 

TRANSPOSE OF H 

COUNTER 

4X4 IDE.NTITY MATRIX 



4 ~ 



c 


J 


= 


COUNTER 


c 


K 


= 


ITERATION INTERVAL 


c 


LPKK 


= 


STATE COVARIANCE MATRIX AFTER PREVIOUS 


c 






OBSERVATIONS 


c 


LPKKM 1 


= 


A PRIORI STATE COVARIANCE ESTIMATE 


c 


LXKK 


= 


STATE ESTIMATE AFTER PREVIOUS OBSERVATIONS 


c 


LXKKMl 


= 


A PRIORI STATE ESTIMATE 


c 


Ml, M2 


= 


AVERAGE OF RESIDUALS OVER LAST THREE 


c 






OBSERVATIONS 


c 


PHI 


— 


DISCRETE-TIME STATE TRANSITION MATRIX 


c 


PHIT 


= 


TRANSPOSE OF PHI 


c 


PI 


= 


3. 141592654 


c 


PKK 


= 


ESTIMATION ERROR COVARIANCE MATRIX, P(K|K) 


c 


PKKS 


= 


SMOOTHED ERROR COVARIANCE MATRIX 


c 


PKKMl 


— 


PREDICTED ESTIMATION ERROR COVARIANCE 


c 






MATRIX, P(K|K-1) 


c 


PKKMIS 


= 


PREDICTED ERROR COVARIANCE MATRIX FOR 


c 






SMOOTHING, P(K+1|K) 


c 


IPKKMIS 


= 


INVERSE OF PKKMIS 


c 


PSS 


= 


ERROR COVARIANCE MATRIX FOR 


c 






SMOOTHING, P(K|K) 


c 


R 


= 


MEASUREMENT NOISE COVARIANCE 


c 


RANGE 


= 


DISTANCE FROM SENSOR TO A PRIORI TARGET 


c 






POSITION 


c 


RTOD 


= 


RADIAN TO DEGREE CONVERSION FACTOR 


c 


SPD 


= 


ESTIMATED TARGET SPEED IN KNOTS 


c 


TEMP 


= 


TEMPORARY STORAGE MATRICES USED IN 


c 






MATRIX OPERATIONS 


c 


VARE 




VARIANCE OF RESIDUALS PROCESS 


c 


XDIFF 


= 


DISTANCE IN X DIRECTION FROM SENSOR TO 


c 






A PRIORI TARGET POSITION 


c 


XKK 


= 


ESTIMATED TARGET STATE VECTOR, X(K|K) 


c 


XKKS 


- 


SMOOTHED TARGET STATE VECTOR 


c 


XKKMl 


= 


PREDICTED TARGET STATE VECTOR, 


c 






X(K|K-1) 


c 


XKKM IS 


= 


PREDICTED TARGET STATE VECTOR FOR 


c 






SMOTHING, X(K+1|K) 


c 


XPOS 


= 


ESTIMATED TARGET POSITION IN X 


c 






DIRECTION 


c 


XS 


= 


SENSOR POSITION IN X DIRECTION 


c 


XSS 


=r 


TARGET STATE VECTOR FOR SMOOTHING, 


c 






X(K|K) 


c 


XT 


= 


TRUE TARGET POSITION IN X DIRECTION 


c 


YD IFF 


= 


DISTANCE IN Y DIRECTION FROM SENSOR TO 


c 






A PRIORI TARGET POSITION 


c 


ypos 


= 


ESTIMATED TARGET POSITION IN Y DIRECTION 


c 


YS 


= 


SENSOR POSITION IN Y DIRECTION 


c 


YT 


= 


TRUE TARGET POSITION IN Y DIRECTION 


c 


ZX 


= 


OBSERVED POSITION IN X DIRECTION 


c 


ZY 


= 


OBSERVED POSITION IN Y DIRECTION 



C VARIABLE DECLARATION’S 
CHARACTER-'a A,B 



RE AL--’- 4 XKK (4,1), XKKM 1(4,1), LPKKM 1(4,4), LXKKMl ( 4,1) 

REAL-->4 H(2,4) ,HT(4,2) ,G(4,2) ,TEMP1(2,1) ,TEMP2(2,4) ,TEMP3(2,1) 



AS 



REAL*4 TEMP4(4,2) ,TEMP5(4,1) ,TEMP6(4,4) ,TEMP7(4,4) 

REAL--^4 PKK(4,4) ,PKKM1(4,4) ,Z(2,1) 

REAL*4 LXKK(4,1) ,LPKK(4,4),XS(10) ,YS(10) ,DBRG(10) ,BRG 
REAL*4 PHI ( 4 , 4 ) , PHIT( 4 , 4 ) , IMAT( 4 , 4 ) , XT , YT 
REAL*4 GATE1,E(2,1),VARE(2,2) ,IVARE(2,2) 

REAL-’-4 DT , DTP , XD IFF , YDIFF , RANGE , XS 1 , YS 1 , BRG 1 , BRKKM 1 

REAL*4 DATE , HR , MN , DAT , LONG , TOTIM ,TIME , TIMEMl , DATE 1 

REAL*4 OBSERRC 300 ) , FAC 1 , S IGTH2 , S IGVT2 , R( 2 , 2 ) , ETOTAL , EAVG , RTOD 

RE AL*4 X2 , YS 2 , BRG2 , ZX , Z Y , M 1 , E 1 , E IM 1 , E 1M2 , DTOR , TRKERR( 300) 

REAL*4 M2,E2,E2M1,E2M2,G11,G13,G21,G23,ZXM1,ZYM1 

REAL*4 XKKS( 4 ,1,300), PKKS( 4,4,300) 

RE AL*4 XNNM 1(4,1),XSS(4,1), XKKM 1 S ( 4 , 1 ) 

REAL*4 PNNM1(4,4),PSS(4,4) ,PKKH1S(4,4) ,IPKKM1S(4,4) 

REAL’>4 AK(4,4),AKT(4,4) ,II(4,4),STRKERR(300) ,DTS(300) 

REAL*4 TEMP 1 S ( 4 , 4 ) , TEMP2S ( 4,1), TEMP3S ( 4,1) 

REAL*4 TEMP4S(4,4) ,TEMP5S(4,4) ,TEMP6S(4,4) 

REAL*4 AS , ASA , ASL , NAV , MET 
INTEGER-’-2 NP 
INTEGER---- , PCN 
C OPEN OUTPUT DATA FILES 

OPEN(UNIT=2 ,FILE=' STORMl. DAT' , STATUS= ' OLD ' ) 

OPEN(UNIT=3 ,FILE =' OUTDATA. DAT' , STATUS=' NEW' ) 
0PEN(UNIT=4,FILE='TRUDATA. DAT' , STATUS= ' NEV.’ ' ) 
OPEN(UNIT=5,FILE='FILDATA. DAT' , STATUS=' NEW' ) 
OPEN(UNIT=6,FILE='SMDATA. DAT' ,STATUS='NE\v' ) 

OPEN(UNIT=7 ,FILE='ELLIPDAT. DAT' , STATUS=' NEW' ) 
OPEN(UNIT=S,FILE=' MATRIX. DAT' , STATUS=' NEW' ) 

OPEN(UNIT=9 ,FILE='ERRDATA. DAT' , STATUS=' NEW' ) 
OPEN(UNIT=10,FILE='ERRSDATA. DAT' , STATUS= ' NEW ' ) 

C RADIAN/DEGREE CONVERSION FACTORS 
RTOD=57. 29577951 
DTOR=0. 01745293 

C COMPUTE 4X4 IDE.NTITY MATRIX 
DO 5 1=1,4 
DO 5 J=1.4 
IF (I.EQ. J) THEN 

IMAT(I,J)=1. 0 

ELSE 

IMAT(I,J)=0. 0 

E.NDIF 

5 CONTINUE 

DO 6 1=1,2 
DO 6 J=l,4 
H(I,J)=0. 0 

6 CONTINUE 
H(l,l)=l. 0 
K(2,3)=l. 0 

C INITIALIZE TIME COUNTER 
TOTTIM=0. 0 
T I MEM 1=0. 0 
NP=0 

C INITIALIZE COUNTER FOR MANEUVER GATE 
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E1M1=0. 0 
E1M2=0. 0 

C COMPUTE BEARING MEASUREMENT COVARIANCE 
C BEARING ERROR STANDARD DEVIATION = 1 NM 

\vRITE(*,*) 'FILTERING OBSERVED DATA WITH KALMAN FILTER' 
WR I TE ( * , * ) ' **-=====, v*,v ' 

810 RE AD (2, 1001, END=8 0 0 ) D ATE , HR , MN , LAT , A , LONG , B , PCN , NAV , MET 

C SATELLITE DATA FOR MEASUREMENT NOISE COV. MATRIX VALUES 
IF(PCN. EQ. 1)THEN 
AS=256. 0 

ELSEIF(PCN. EQ. 3)THEN 
AS=900. 0 

ELSEIF(PCN. EQ. 5)THEN 
AS=1600. 0 
C RADAR DATA 

ELSEIF(PCN. EQ. 2)THEN 
AS=100. 0 

ELSEIF(PCN.EQ. 4)TKEN 
AS=225. 0 

ELSEIF(PCN. EQ. 6)THEN 
AS=625. 0 
C AIRCRAFT DATA 
ELSE 

AS=( (NAV)*'''2+(MET)*-"'2)**0. 5 
END IF 
R( 1,1)=AS 
R(l,2)=0. 0 
R(2,l)=0. 0 
R(2,2)=AS 



C READ IN OBSERVATION PACKET (DATE, TIME, LAT, LONG) 
C DT=TIME(K)-TIME(K-1) 



C RE AD ( 2 , 1 0 0 1 , END=S 00 ) DATE , HR , MN , LAT , A , LONG , B 

1001 FORMAT(F6. 0,F2. 0,F2. 0,F3. 0,A1,F4. 0 , A1 , 1 1 , 2(F2. 0)) 

NP=NP+1 

MN=MN/60. 0 
LAT=LAT/10 
L0NG=L0NG/10 
TIME=HR+MN 

C WRITE (3,1) DATE, HR, MN, LAT, A, LONG, B 

1 F0RMAT(1X,F7. 0,4X,F3. 0 , 1X,F6. 4 ,6X ,F4. 1,A1,3X,F5. 1,A1) 



IF (NP. EO. 1) THEN 
DATE 1=D ATE 
TIMEM1=TIME 
E.NDIF 
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IF (DATE. NE. DATE 1) THEN 
TIME=TIME+24 
DT=TIME-TIMEM1 
TIME=TIME-24 
ELSE 

DT=TIME-TIMEM1 
END IF 



DTF=DT*60. 0 

DTS(NP)=DT 

TOTTIM=TOTTIM+DT 

C WRITE (3,2) TIME,TOTTIM,DT 

2 F0RMAT(1X,F7.4,5X,F6. 2,5X,F6. 2) 

CALL FINDPHI(PHI,DT) 

Z(1,1)=L0NG 

Z(2,1)=LAT 

ZX=LONG 

ZY=LAT 

IF(NP. EQ. 1) THEN 

CALL INIT(LONG.LAT,XKK,PKK) 

C WRITE(-’',-'^)'X(0i0,0): ' 

DO 601 1=1,4 
LXKK(I,1)=XKK(I,1) 

WR I TE ( 3 , * ) ' ************* ’ 
WRITE ( 3 , ’•- ) ( XKK( 1 , 1 ) , 1= 1 , 4 ) 
CO.NTINUE 



WRITE(3,^--)'P(0|0,0): ' 
DO 602 1=1,4 
DO 602 J=l,4 
LPKK(I,J)=PKK(I,J) 
WRITE(3,401)PKK(I,J) 

01 F0RMAT(4F14. 4) 

02 CONTINUE 



E.NDIF 



PROJECT AHEAD STATE A.ND ERROR COVARIANCE ESTIMATES 
X(K+11K) = PHI X(K|K) 

CALL MATMUL( PH I , XKK , 4 , 4 , 1 , XKKM 1 ) 



603 



WRITE (•->,-'>) 'X( ' ,TIME, ' | ’ ,TIMEM1, ' ,0); ' 
DO 603 1=1,4 

WR I TE ( 3 , * ) ( XKKM 1(1,1), 1=1, 4) 

WR I TE ( 3 , * ) ' **************** ' 

LXKKM 1(1,1) =XKKM 1(1,1) 

CONTINUE 



C P(K41]K) = (PHI * F(K|K) * PHIT) + Q 
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CALL MATRAN(PHI,PHIT,4,4) 

CALL MATMULC PHI , PKK , 4 , 4 , 4 , TEMP6 ) 

CALL MATMULC TEMP6 , PHIT ,4,4, 4 , TEMP 7 ) 
CALL GETQCQ) 

CALL MATADDC TEMP 7 , Q , 4 , 4 , 1 , PKKM 1 ) 

DO 408 1=1,4 
DO 408 J=l,4 

LPKKMK I , J)=PKKM1( I , J) 

408 CONTINUE 

C WRITEC*,^--) ’P( ’ ,TIME, ’ | ’ ,TIMEM1, ' ,0): ' 

DO 604 1=1,4 

C WRITE(3,402)(PKKM1(I,J) ,J=1,4) 

402 F0RMATC4F14. 4) 

604 CONTINUE 



204 CONTINUE 



C COMPUTE OBSERVATION RESIDUAL 
C E=Z(K)-H*X(K|K-1) 

CALL MATMULC H , XKKMl ,2,4,1, TEMPI ) 

CALL MATSUBCZ,TEMP1,2,1,E) 

C COMPUTE VARIANCE OF RESIDUALS SEQUENCE 
C AND ADAPTIVE GATE VALUE 
C VARCE)=H*PKKM1*HT+R 

CALL MATRANCH,HT,2,4) 

CALL MATMULC H , PKKM 1 , 2 , 4 , 4 , TEMP2 ) 

CALL MATMULC TEMP2 , HT , 2 , 4 , 2 , TEMP3 ) 

CALL MATADDC TEMP 3 , R , 2 , 2 , 1 , VARE) 

C WRITEC 3, 'VARIANCE OF RESIDUALS = ’ ,VARE 

C GATE1=1. 5*SQRTCVARE) 

C COMPUTE KALMAN GAIN MATRIX 
C G=PKKM 1*HT--' C H^'PKKM 1 ->HT+R ) ** - 1 

CALL MATRANCH,KT,2,4) 

CALL MATMULC PKKM 1 , HT , 4 , 4 , 2 , TEMP4 ) 

CALL MATI NV ( VARE ,2.1 VARE ) 

CALL MATMULC TEMP4 , 1 VARE , 4 , 2 , 2 , G ) 

C WR ITE C 3 , ) ’ PKKM 1 ’'HT = ' 

DO 414 1=1,4 

C WRITEC3,*)TEMP4CI,1) 

414 CONTINUE 

C VRITEC3,--'-) 'G =' 

DO 613 1=1,4 

C WRITEC3,-’0GCI,1) 

613 CONTINUE 

C IF CL. EQ. 1) THEN 

C Gll=GCl,l) 

C G13=Gl3,1) 

n 



ELSE 



C G21=G(1,1) 

C G23=G(3,1) 

C END IF 

C COMPUTE UPDATED ESTIMATE 

C X(K|K)=X(K|K-1)+G'--E, WHERE E=Z(K) -H*X(K| K-1) 
CALL MATMULC G , E , 4 , 2 , 1 , TEMPS ) 

CALL MAT ADD (TEMPS , XKKM 1 , 4 , 1 , 1 , XKK ) 

C WRITE(3,*)'X( ' ,TIME/ 1 ' .TIME, ’ ,L,' ): ’ 

DO 60S I=-l,4 

C VRITE(3,’-)XKK(I,1) 

60S CONTINUE 



C COMPUTE UPDATED ERROR COVARIANCE MATRIX 
C P(K1K)=(I - G-VH)’>P(K|K-1) 

CALL MATMUL(G,K,4,2,4,TEMP6) 

CALL MATSUB(IMAT,TEMP6,4,4,TEMP7) 

CALL MATMULC TEMP 7 , PKKM 1 , 4 , 4 , 4 , PKK ) 

C WRITE(3,*)'P( ' .TIME, ’ 1 ' .TIME, ' . ' ,L. ' ' 

DO 606 1=1,4 

C WRITE(3,406)(PKK(I,J) ,J=1,4) 

406 F0RMT(4F14. 4) 

606 CONTINUE 



C THESE STATEMENTS ARE FOR THE SMOOTHING ALGORITHM 

DO 620 1=1,4 
XKKS(I,1,NP)=XKK(I,1) 

620 CONTINUE 

DO 630 1=1,4 
DO 630 J=i , 4 

PKKS(I,J,NP)=PKK(I,J) 

630 CONTINUE 

C COMPUTE TRUE TRACKING ERROR 
ASA=XKK(1,1) 

ASL=XKK(3, 1) 

TRKERRC NP ) =SQRT( ( LAT- ASA) **2+( LONG - ASL) **2 ) 

C COMPUTE OBSERVATION ERROR 

C OBSERR(NP)=SQRT( (ASLAT-ZX)**2+(ASLONG-ZY)**2) 

C SAVE LATEST RESIDUALS FOR AVERAGING 
C E1=E 



COMPUTE THE AVERAGE RESIDUAL OVER THE PAST THREE OBSERVATIONS 
Ml=(El+ElMl+ElM2)/3 

C WRITE(’S-")'PAST THREE RESIDUALS FOR SENSOR 1 ARE : ' ,E1 ,E1M1 ,E1M2 

C WRITEf-'-'.-'O 'BEARING AVERAGE OF SENSOR 1 = ' ,M] 

C WR I TE(*,---V MANEUVER GATE FOR SENSOR 1 = ’ ,GATE1 



53 



o o o o 



E1M2=E1M1 
E1M1=E1 

COMPUTE ERROR ELLIPSE DATA 

CALL ELLI P( XKK( 1 , 1 ) , XKK( 3 , 1 ) , PKK( 1 , 1 ) , PKK( 3 , 3 ) , PKK( 1,3)) 

C COMPUTE ESTIMATED X-Y POSITION, COURSE, AND SPEED 
XPOS=XKK(l,l) 

YPOS=XKK(3,l) 

IF (XKK(2,1).EQ. 0 .AND. XKK(4, 1). EQ. 0) THEN 
HDG=0. 0 

ELSE 

HDG=RT0D*ATAN2( XKK( 2,1), XKK( 4,1)) 

ENDIF 

IF (HDG. LT, 0.0) HDG=HDG+360 
SPD=60*SQRT(XKK(2, l)-''-'-2+XKK(4, 1)**2) 

C WRITE(" ,-••) 'FILTERED DATA FOR DATA POINT* ,NP 

V;RITE(3,*) 'filtered DATA FOR DATA POINT* ,NP 
C V;RITE(-- ,"■) 'TIME X POS Y POS HEADING SPEED* 

WRITE(3,-'-') 'TIME X POS Y POS HEADING SPEED* 

C WRITEC-'- ,*)TOTTIM,XPOS , YPOS ,HDG, SPD 

VRITE( 3 ,--'OTOTTIM , XPOS , YPOS ,HDG , SPD 
WRITE ( 4 , -> ) TOTTIM , ZX , Z Y 
WRITE( 5 ,•■•) TOTTIM , XPOS , YPOS , PKK( 1,1) 

WRI TE ( 9 , ’> ) N? , TRKERR ( NP ) 

1002 FORMAT(1X,5F10. 3) 

1003 F0RMAT(1X,F6. 2,3X,F10. 1,2X,F11. 1,3X,F8. 1,3X,F8. 1) 

1004 F0RMAT(1X,F6. 2,3(F8. 1,2X)) 

C COMPARE BEARING ERRORS TO MANEUVER DETECTION GATES 

IF ((ABS(Ml). GT. (GATED)') THEN 

WR I TE(^'-,--D MANEUVER DETECTION ***' 

C WRITE(3,-D ’-•** MA.NEUVER DETECTION 

CALL RE INIT( DT , ZX , Z Y , ZXM 1 , ZYM 1 , LPKKM 1 , XKKM 1 , PKKM 1 ) 
E1M1=0. 0 
E1M2=0. 0 
GOTO 204 

ENDIF 

TIMEM1=TIME 

DATE1=DATE 

ZXM1=ZX 

ZYM1=ZY 

GOTO 810 

C THIS IS WHERE THE SMOOTHI.NG ALGORITHM STARTS 
C FIXED INTERVAL SMOOTHING 

800 WRITE -•■•-) 'SMOOTHING FILTERED DATA WITH A* 

WRITE(-D'--) 'FIXED I.NTERVAL SMOOTHING ALGORITHM* 

WRITEC ) ' v.--v,v=====-.>-;.-.v ' 
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DO 1000 KK=1,NP-1 
K=NP-KK 



DT=DTS(K+1) 

TIME=TIMEM1-DT 
CALL FINDPHI(PHI,DT) 

DO 901 1=1,4 
XSS(I,1)=XKKS(I,1,K) 

901 CONTINUE 

DO 902 1=1,4 
DO 902 J=l,4 
PSS(I,J)=PKKS(I,J,K) 

902 CONTINUE 



C CALCULATE THE PREDICTED STATE AND ERROR COVARIANCE MATRICES 
C X(K+1|K)=PHI*X(K|K) 

CALL MATMUL ( PHI ,XSS , 4 ,4 , 1 ,XKKM1S) 

C P( K+1 1 K)=PHI*F(K I K) -•••PHIT+Q 

CALL MATRAN (PHI ,PHIT,4,4) 

CALL MATMULC PHI , PSS , 4 , 4 , 4 , TEMP6 ) 

CALL MATMULC TEMPb , PHIT , 4 , 4 , 4 , TEMP7 ) 

CALL GETQ(Q) 

CALL MATADDC TEMP7 , Q , 4 , 4 , 1 , PKKMIS ) 



C CALCULATE THE SMOOTHING FILTER GAIN MATRIX 
C AK=F ( K I K ) --■^PH I r-'- 1 N V “ P ( K+ 1 1 K ) 

CALL MATINV ( PKKMIS , 4 , IPKKMIS) 

CALL MATMUL ( PKKM 1 S , I PKKM IS, 4, 4, 4, II) 
CALL MATMUL ( PSS , PHIT,4 , 4 .4 ,TEMP1S) 
CALL MATMUL (TEMPIS , IPKKMIS , 4 ,4 , 4 , AK) 

DO 904 1=1,4 

XNNM 1(1,1) =XKKS ( I , 1 , K+ 1 ) 

904 CONTINUE 

C CALCULATE THE SMOOTHED STATE ESTIMATE 
C XKKS=X(KiK)+AK''-(X(K+l |N)-X(K+1|K) 

CALL MATSUE (XNNMl ,XKKM1S , 4 , 1 ,TEMP2S) 
CALL MATMUL (AK,TEMP2S ,4,4 , 1 .TEMPOS) 
CALL MATADD ( XSS, TEMPOS, 4, 1,K,XKKS) 

DO 906 1=1,4 
DO 906 J=l,4 

PNNM 1 ( I , J ) =PKKS ( I , J , K+ 1 ) 

906 CONTINUE 

C CALCULATE THE SMOOTHED COVARIANCE MATRIX 
C PKKS=P ( K ! K ) +AK* [ P ( K+ 1 1 N ) - P ( K+ 1 1 K ) ] •••AKT 

CALL MATSUB (PNNMl .PKKMIS ,4,4 ,TEMP4S) 
CALL MATRAN (AK,AKT,4,4) 

CALL MATMUL ( AK ,TEMP4S ,4 , 4 ,4 ,TEMP5S) 



CALL MATMUL (TEMP5S,AKT,4,4,4,TEMP6S) 

CALL MATADD (PSS,TEMP6S,4,4,K,PKKS) 

C COMPUTE ESTIMATED X-Y POSITION, COURSE, AND SPEED 
SXPOS=XKKS(l,l,K) 

SYPOS=XKKS(3,l,K) 

IF (XKKS(2,1,K).EQ. 0 .AND. XKKS(4, 1,K). EQ. 0) THEN 
SHDG=0. 0 

ELSE 

SHDG=RTOD*ATAN2(XKKS( 2 , 1 , K) ,XKKS( 4 , 1 , K) ) 

ENDIF 

IF (SHDG. LT. 0. 0) SHDG=SHDG+360 
SSPD=60*SQRT(XKKS(2, 1 ,K)**2+XKKS( 4 , 1 ,K)**2) 

C WRITEC*,-'-) 'SMOOTHED DATA FOR DATA POINT’ ,K 

WRITE(3,*) 'SMOOTHED DATA FOR DATA POINT' ,K 
C WRITE(*,*) 'TIME X POS Y POS HEADING SPEED' 

WRITE(3,’0 'TIME X POS Y POS HEADING SPEED' 

C WRITE( * ,*)TOTTIM , SXPOS , SYPOS , SHDG , SSPD 

V;RITE( 3 ,-'-)T0TTIM , SXPOS , SYPOS , SHDG , SSPD 
1010 FORMAT(1X,5F10. 3) 

1020 F0RMAT(1X,F6. 2,3X,F10. 1,2X,F11. 1,3X,F8. 1,3X,F8. 1) 

1030 FORMATC IX, F6. 2,3(F8. 1,2X)) 

TIMEM1=TIME 
1000 CONTINUE 

C CL0SE(UNIT=4) 

C CALCULATE THE SMOOTHED TRACKING ERROR 
C 0PEN(UNIT=4,FILE='TRUDATA. DAT' ,STATUS=' OLD' ) 

DO 1100 K=1,NP 
SXPOS=XKKS(l,l,K) 

SYPOS=XKKS(3, 1,K) 

C RE.4D( 4,1001) DATE , HR , MN , LAT , A , LONG , B , PCN 

STRKERR(K)=SQRT((LAT-SXPOS)**2+(LONG-SYPOS)**2) 

WRITE(6, 1120) K, SXPOS, SYPOS, PKKS( 1, 1,K) 

WRITEC 10,*)K,STRKERR(K) 

1100 CONTINUE 

1110 FORMATC 14, 2F8. 1) 

1120 FORMATC 14, 3( FS. 1,2X)) 

1130 F0RMATCI4,3FS. 1) 

CLOSECUNIT=2) 

CLOSE C UN IT=3) 

CL0SECUNIT=4) 

CLOSECUNIT=5) 

CLOSECUNIT=6) 

CLOSEC'UNIT=7) 

CLOSE CUNIT=8) 

CLOSECUNIT=9) 

CLOSECUNIT=10) 

WR I TEC ••',*) 'FILTERED & SMOOTHED OUTPUT DATA IS LOCATED IN THE 
WRITEC '•■,") 'D.ATA FILE OUTDATA.DAT. FOR GRAPHIC RESULTS,' 
WRITEC-S-O 'ensure 0BSDATA.DAT, FILDATA.DAT, & SMDATA.DAT ARE 



WRITE(*,*) 'IN THE MATLAB SUB-DIRECTORY AND RUN THE MATLAB' 
VRITE(*,*) 'M-FILE STORM2.M’ 

STOP 

END 



C SUBROUTINES 

QV:VfVrVrVrVrV:*V:yc*V?VcVrVrVT**Vc‘VV?VfV'VcVtVcVrVcVrVr:VV?yfVrVrV?VrVc*Vf'>V'>VV?Vf'5V^V';VVc'>V*VrVc*Vr*VcVrV?VfVr*VrVr“V 



SUBROUTINE FINDPHK PHI ,DT) 

Q V-VrV*'VrVrVrV?V?Vc*;VVrVrVrVr^VVrV»-*VrVcVrV.-Vf:VVrVr*^V*VrVfVcV?V?Vr**Vr':V*Vr*'>V'>VVr*vVVrVrVry:V-%-Vc 

C COMPUTES THE VALUES OF THE PHI MATRIX 

Q Vc Vc “/V -jV Vr V>- * Vr Vr Vr Vc V: Vr Vr Vr Vr Vf Vc Vc Vr Vr Vr Vr V? Vr Vr Vr Vr Vc * Vr Vr Vr Vr Vr Vt * Vr Vc ■) V “V * V * V Vc it it it it it it it it it it 

REAL*4 PHI(4,4),DT 

DO 1501 1=1,4 
DO 1501 J=l,4 
DO 1501 K=l,2 

PHI(I,J)=0. 0 

1501 CONTINUE 

C COMPUTE PHI MATRIX 
DO 1500 1=1,4 
PHI(I,I)=1. 0 
1500 CONTINUE 

PHI(1,2)=DT 

PHI(3,4)=DT 



RETURN 

END 



C 

C 

C 

C 



SUBROUTINE INIT( LONG , LAT , XKK , PKK) 

ititititititititi:i:ititi:itititi:iri:ititirititi:irititititititititititi:i:itititititit 



THIS ROUTINE INITIALIZES THE STATE 
AND ERROR COVARIANCE ESTIMATES 



“V it i: it it it it it it it it it i: it it-. 



it it it it it it it it it it it 



REAL"- 4 XKK( 4,1), PKK( 4,4) 
REAL^>4 LAT, LONG 



C INITIAL STATE ESTIMATE 
XKK(3,1)=LAT 
X?;K(2,1)=0. 0 
XKKfl,l)=LONG 
XKK(4,1)=0. 0 



C INITIAL ERROR COVARIANCE ESTIMATE 
PKK(1,1)=100. 0 
PKK(1,2)=0. 0 
PKK(1,3)=0. 0 
PKK(1,4)=0. 0 
PKK(2.1)=0. 0 
PKK(2,2)=0. 025 



^ ; 



o o o o 



PKK(2,3)=0. 0 
PKK(2,4)=0. 0 
PKK(3,1)=0. 0 
PKK(3,2)=0. 0 
PKK(3,3)=100 
PKK(3,4)=0. 0 
PKK(4,1)=0. 0 
PKK(4,2)=0. 0 
PKK(4,3)=0. 0 
PKK(4,4)=0. 025 

RETURN 

END 

SUBROUTINE GETQ(Q) 

C ROUTINE TO GET Q MATRIX 

REAL----4 Q(4,4) 

DO 100 1=1,4 
DO 100 J=l,4 
Q(I,J)=0. 0 
DO 200 1=1,4 
200 Q(I,I)=100. 

RETURN 

END 



SUBROUTINE REINIT( DT , ZX , ZY , ZXMl , ZYMl , LPKKMl , XKKMl , PKKMl ) 

THIS ROUTINE RE -INITIALIZES THE STATE AND ERROR 
COVARIANCE ESTIMATES 

Vf Vr Vr V? Vr Vr Vf Vr Vr Vr Vr Vr ‘A- Vr Vc Vr Vr Vr Vr Vv' Vc Vr Vr Vr Vr Vr Vr 7V* Vr * * Vr * * Vr V? Vf * Vf Vr Vf Vr Vr Vr Vr 

RE AL*4 DT , XEKM 1(4,1), PKKM 1(4,4) 

REAL---4 ZX , ZY , ZXMl , ZYMl , LPKKMl ( 4,4) 

XDIFF=ZX-ZXM1 
YDIFF=ZY-ZYM1 

XKKM1(1,1)=ZX 
XKKMK2, 1)=XDIFF/DT 
XKKM1(3,1)=ZY 
XKKM 1(4,1) =YD IFF/DT 

C WRITE( 3,*) 'REINITIALIZED STATES ARE:’ 

DO 100 1=1,4 

C WRITE(3,*)XKKM1(I,1) 

100 CONTINUE 

PKKMl ( 1 , 1)=2. 25*LPKKM1( 1,1) 

PKKM1(1,2)=0. 0 

PKKMK 1 , 3)=2. 25*LP?:KM1( 1,3) 



C}Oonn o oooo 



PKKM1(1,4)=0. 0 
PKKM1(2,1)=0. 0 
PKKM1(2,2)=0. 1111 
PKKM1(2,3)=0. 0 
PKKM1(2,4)=0. 0 
PKKM 1 ( 3 , 1 ) =2 . 2 5*LPKKM 1(3,1) 
PKKM1(3,2)=0. 0 
PKKM1(3,3)=2. 25*LPKKM1(3,3) 
PKKM1(3,4)=0. 0 
PKKM1(4,1)=0. 0 
PKKM1(4,2)=0. 0 
PKKM1(4,3)=0. 0 
PKKM1(4,4)=0. 1111 

RETURN 



END 



SUBROUTINE MP( XS 1 , YS 1 , XS2 , YS2 , BRGl , BRG2 , ZX , ZY) 

THIS ROUTINE COMPUTES THE ESTIMATED 
X,Y POSITION OBTAINED FROM MEASUREMENTS 

REAL*4 ZX,ZY 

REAL*4 XS 1 , YS 1 , XS2 , YS 2 , BRG 1 , BRG2 
REAL'H NUMER,DENOM 

INITIAL STATE ESTIMATE 

NUMER=( -YS2*TAN( BRG2 ) )+( YS 1*TAN( BRGl ) ) +XS2-XS 1 
DENOM=TAN( BRGl ) -TAN( BRG2 ) 



ZY=NUMER/DENOM 

ZX=( Z Y - YS 1 ) *TAN( BRG 1 ) +XS 1 

RETURN 



END 



SUBROUTINE ELLIP( XT . YT , P 1 , P3 , P13 ) 

THIS SUBROUTINE COMPUTES ERROR ELLIPSE DATA 
FROM ERROR COVARIANCE DATA 

DIMENSIONS AND DECLARATIONS 

REAL*4 XT,YT,XP(21) ,YP(21) ,A,B,THE1,SIG2X,SIG2Y 
REAL*4 SX , SY , PT , CT , ST , P 1 , P 13 , P3 



A=2->P13 

E=P1-P3 

THE 1=0. 5*ATAN2(A,B) 

A=(Pl+P3)/2 

’ 5 = 0 . 0 

IF (P13. EQ. 0. 0) GOTO 10 
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B=P13/SIN(2. 0*THE1) 

10 SIG2X=ABS(A+B) 

SIG2Y=ABS(A-B) 

SX=SIG2X**0. 5 
SY=SIG2Y**0. 5 
PT=3. 141592654/10 
CT=C0S(THE1) 

ST=SIN(THE1) 

DO 100 IE=1,21 

XP( IE ) =SX*COS ( PT* IE ) *CT - S Y*S IN( PT* IE ) *ST+XT 
YP( IE )=SX’--COS ( PT*IE ) *ST+S Y*S IN( PT-'IE ) *CT+YT 
WRITE(7,''-)XP(IE) ,CHAR(9) ,YP(IE) 

100 CONTINUE 
RETURN 
END 



SUBROUTINE MATMUL( A,B,L,M,N,C) 

C THIS ROUTINE MULTIPLIES T\v'0 MATRICES TOGETHER 

C “ C(L,N) = A(L,M) * B(M,N) 

C DIMENSIONS AND DECLARATIONS 

REAL*4 A(L,M) ,B(M,N) ,C(L,N) 

DO 10 1=1, L 
DO 10 J=1,N 
C(I,J)=0. 0 
10 CONTINUE 

DO 100 1= 1,L 
DO 100 J= 1,N 
DO 100 K= 1,M 

C(I,J) = C(I,J) + A(I,K)-’-B(K,J) 

100 CONTINUE 

RETURN 

END 



SUBROUTINE MATRAN(A,B,N,M) 

Q Vf Vv' Vr Vr Vr * V -V Vr Vr ‘ V Vr Vr i: 'V Vr :;V Vr Vr Vr Vr Vr Vr V- *V Vr Vr Vr “iV *V Vr Vc Vf Vc ‘.V Vc 

C THIS ROUTINE TRANSPOSES A MATRIX 

C ° B(M,N) = A'(N,M) 

C DIMENSIONS AND DECLARATIONS 

REAL-'--4 A(N,M), B(M,N) 

DO 100 1= 1,N 
DO 100 J= 1,M 
B(J,I) = A(I,J) 

100 CONTINUE 
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RETURN 



END 



C •>v*>v*** 




SUBROUTINE MATSCL(Q,A,N,M,C) 



I 



c 



C 

c 



THIS ROUTINE MULTIPLIES A MATRIX WITH A SCALAR 
° C(N,M) = Q * A(N,M) 




DIMENSIONS AND DECLARATIONS 



REAL---4 A(N,M), C(N,M), Q 

DO 100 I = 1,N 
DO 100 J = 1,M 
C(I,J) = Q^'^Ad.J) 

100 CONTINUE 

RETURN 

END 



SUBROUTINE MATSUB( A, B ,N,M,C) 

C THIS ROUTINE SUBTRACTS T\vO MATRICES 

C “ C(N,M) = A(N,M) - B(N,M) 

C DIMENSIONS AND DECLARATIONS 

RE AL---4 A( N , M ) , B ( N , M ) , C ( N , M ) 

DO 100 I = 1,N 

DO 100 J = 1,M 

C(I,J)=A(I,J)-B(I,J) 

100 CONTINUE 

RETURN 

ExND 

SUBROUTINE MATADD ( A , B , N , M , L d ) 

C THIS ROUTINE ADDS V,‘10 MATRICES 

C “ C(N,M) = A(N,M) + B(N,M) 

C DIMENSIONS AND DECLARATIONS 

REAL*4 A(N,M) ,B(N,M) ,C(N’,N,L) 

DO 100 I = 1,N 

DO 100 J = 1,M 

C(I,J.L)=A(I,J)+B(I,J) 

100 CONTINUE 

RETURN 



END 
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c 

c 

c 

c 

c 

c 






100 



SUBROUTINE HATINV (A,N,C) 

THIS ROUTINE COMPUTES THE INVERSE OF 
A MATRIX 

C(N,N)=INV [A(N,N)] 

DIMENSIONS AND DECLARATIONS 
REAL*4 A(N,N),C(N,N),D(20,20) 

DO 100 I = 1,N 
DO 100 J = 1,N 
D(I,J)=A(I,J) 



DO 115 1=1, N 
DO 115 J=N+1,2-'N 
115 D(I,J)=0.0 



DO 120 1=1, N 
J=I+N 

120 D(I,J)=1.0 



DO 240 K=1,N 
M=K+1 

IF (K. EQ. N) GOTO 180 
L=K 

DO 140 I=M,N 

140 IF (ABS(D(I,K)). GT. ABS(D(L,K))) L=I 

IF (L. EQ.K) GOTO 180 



DO 160 J=K,2*N 
TEMP=D(K, j) 
D(K,J)=D(L,J) 
160 D(L,J)=TEMP 



180 DO 185 J=M,2-'-N 

185 D(K,J)=D(K,J)/D(K,K) 

IF (K. EQ. 1) GOTO 220 
M1=K-1 

DO 200 1=1, Ml 
DO 200 J=M,2*N 

200 D(I,J)=D(I,J)-D(I,K)*D(K,J) 

IF (K.EQ. N) GOTO 260 

220 DO 240 I=M,N 

DO 240 J=M,2*N 

240 D(I,J)=D(I,J)-D(I,K)*D(K,J) 

260 DO 265 1=1, N 

DO 265 J=1,N 
K=JxN 

265 C(I,J)=D(I,K) 



RETURN- 

END 
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APPENDIX B. WIND.FOR 



This a listing of the WIND. TOR micro computer program used to generate the data 
for the storm wind speed tracks presented in this thesis. In order to run this program, 
the WIND01.D.\T file must be available. 



C 









* * V? * Vr •jV Vr Vf ->V * ■ 



* 



C ***VARIABLE DEFINITIONS*** 



C 

C 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

n 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 



AK 

ART 

BRG 

BRKKMl 

DBRG 

DT 

DIOR 

E1,E2 

E1M1,E2M1 

E1M2,E2M2 

FACl 

G 

GATEl 

H 

HDG 

KT 

I 

I MAT 

J 

K 

LPKK 

LPKKMl 

LXKK 

LXKRMl 

Ml, M2 

PHI 

PHIT 

PI 

PKK 

PKKS 

FKKMl 

PKKMIS 

IPKKMIS 

PSS 

RANGE 

RTOD 

SFD 

TEMP 



SMOOTHING FILTER GAIN MATRIX 
TRANSPOSE OF AK 

MEASURED TARGET BEARING IN RADIANS 
PREDICTED TARGET BEARING MEASUREMENT IN RADIANS 
BRG(K|K-1) 

MEASURED TARGET BEARING IN DEGREES 

TIME DELAY BETV^TEN OBSERVATIONS ,T(K) - T(K1) 

DEGREE TO RADIAN COxNVERSION FACTOR 
MEASUREMENT RESIDUAL, Z(K) - H(X(K|K-1)) 

MEASUREMENT RESIDUAL AT PREVIOUS OBSERVATION 
MEASUREMENT RESIDUAL TWO OBSERVATIONS PREVIOUS 
RECIPROCAL OF VARE 
KALMAN GAIN VECTOR 

1.5*STA.NDARD DEVIATION OF RESIDUAL PROCESS, USED AS i 
GATE IN MANEUVER DETECTION 
MEASUREMENT MATRIX 
ESTIMATED TARGET HEADING IN DEGREES 
TRANSPOSE OF H 
COUNTER 

4X4 IDENTITY MATRIX 
COUNTER 

ITERATION INTERVAL 

STATE COVARIANCE MATRIX AFTER PREVIOUS OBSERVATIONS 
A PRIORI STATE COVARIANCE ESTIMATE 
STATE ESTI.MATE AFTER PREVIOUS OBSERVATIONS 
A PRIORI STATE ESTIMATE 

AVERAGE OF RESIDUALS OVER LAST THREE OBSERVATIONS 
DISCRETE -TIME STATE TRANSITION MATRIX 
TRANSPOSE OF PHI 
3. 141592654 

ESTIMATION ERROR COVARIANCE MATRIX, P(K|K) 

SMOOTHED ERROR COVARIANCE MATRIX 

PREDICTED ESTIMATION ERROR COVARIANCE MATRIX, P(K|K-1 
PREDICTED ERROR COVARIANCE MATRIX FOR SMOOTHING, P(K4 
INVERSE OF PKKMIS 

ERROR COVARIA.NCE MATRIX FOR SMOOTHING, P(K|K) 
MEASUREMENT NOISE COVARIANCE 

DISTANCE FROM SENSOR TO A PRIORI TARGET POSITION 
RADIAN TO DEGREE CONVERSION FACTOR 
ESTIMATED TARGET SPEED IN KNOTS 
TEMPORARY STORAGE MATRICES USED IN MATRIX 
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VARE 

XDIFF 



XKK 

XKKS 

XKKMl 

XKKMIS 

XPOS 

XS 

XSS 

XT 

YD IFF 

YPOS 

YS 

YT 

ZX 

ZY 



OPERATIONS 

VARIANCE OF RESIDUALS PROCESS 

DISTANCE IN X DIRECTION FROM SENSOR TO A PRIORI 
TARGET POSITION 

ESTIMATED TARGET STATE VECTOR, X(K|K) 

SMOOTHED TARGET STATE VECTOR 

PREDICTED TARGET STATE VECTOR, X(K|K-1) 

PREDICTED TARGET STATE VECTOR FOR SMMOTHING, X(I 

ESTIMATED TARGET POSITION IN X DIRECTION 

SENSOR POSITION IN X DIRECTION 

TARGET STATE VECTOR FOR SMOOTHING, X(KlK) 

TRUE TARGET POSITION IN X DIRECTION 

DISTANCE IN Y DIRECTION FROM SENSOR TO A PRIORI 

TARGET POSITION 

ESTIMATED TARGET POSITION IN Y DIRECTION 
SENSOR POSITION IN Y DIRECTION 
TRUE TARGET POSITION IN Y DIRECTION 
OBSERVED POSITION IN X DIRECTION 
OBSERVED POSITION IN Y DIRECTION 



C VARIABLE DECLARATIONS 
CHARACTER*! A,B 



REAL*4 XKK( 2,1), XKKM 1(2,1), LPKKMl ( 2,2), LXKKMl ( 2,1) 

REAL*4 H(2,2) ,HT(2,2) ,G(2,1) ,TEMP1(2,1) ,TEMP2( 2 , 2) ,TEMP3( 2 , 1 ) 
REAL*4 TEMP4( 2,2), TEMPS (2,1), TEMP6( 2,2), TEMP7 ( 2,2) 

RE AL*4 PKK( 2 , 2 ) , PKKM 1(2,2),Z(1,1) 

REAL*4 LXKK(2 , 1) ,LPKK(2 ,2) ,XS( 10) ,YS( 10) ,DBRG( 10) ,BRG 
REAL*4 PHI ( 2 , 2 ) , PHIT( 2 , 2 ) , IMAT( 2 , 2 ) , XT , YT 
REAL*4 GATE 1 , E( 2 , 1 ) , VARE( 2,2), IVARE( 2,2) 

RE AL*4 DT , DTF , XDIFF , YD I FF , RANGE ,XS1,YS1,BRG1, BRKKM 1 

REAL*4 DATE , HR , MN , LAT , LONG , TOTIM , TIME , TIMEM 1 , DATE 1 

REAL*4 OBSERR( 300 ) , FAC 1 , S IGTH2 , S IGVT2 , R( 2 , 2 ) , ETOTAL , EAVG , RTOD 

REAL*4 X2 , YS2 , BRG2 , ZX , ZY , Ml , El , E IMl ,E 1M2 ,DTOR ,TRKERR( 300) 

REAL*4 M2 , E2 ,E2M1 , E2M2 , G1 1 , G13 , G2 1 , G23 , ZXMl , ZYMl 

REAL*4 XKKS( 2,1,300), PKKS( 2,2,300) 

REAL*4 XNNM 1(2,1),XSS(2,1), XKKM 1 S ( 2 , 1 ) 

REAL*4 PNNM 1(2,2),PSS(2,2), PKKM 1 S ( 2 , 2 ) , I PKKM 1 S ( 2 , 2 ) 

REAL*4 AK( 2 , 2 ) , AKT( 2 , 2 ) , I I ( 2 , 2 ) , STRKERR( 300 ) , DTS ( 300 ) 

REAL*4 TEMP IS ( 2 , 2 ) , TEMP2S( 2 , 1 ) , TEMP3S( 2,1) 

REAL*4 TEMP4S( 2,2), TEMPS S( 2,2), TEMP6S( 2,2) 

REAL*4 AS , ASA , ASL , WIND , WINDD , NAV , MET 
INTEGER*2 NP,ASIM,K 
INTEGER*, PCN 
C OPEN OUTPUT DATA FILES 

0PEN(UNIT=2,FILE='WIND01. DAT' , STATUS=' OLD ' ) 

OPEN(UNIT=3,FILE ='OUTDATA. DAT’ , STATUS= ’ NEW ' ) 
0PEN(UNIT=4,FILE=’0ESDATA. DAT* , STATUS=’ NEW’ ) 
OPEN(UNIT=S,FILE=’FILDATA. DAT’ ,STATUS=’NEW’ ) 
OPEN(UNIT=6,FILE=’ SMDATA. DAT’ , STATUS=' NEW’ ) 

OPEN(UNIT=8 ,FILE =’ MATRIX. DAT’ , STATUS=’ NEW’ ) 

OPEN(UNIT=9,FILE =’ PALDATA. DAT’ , STATUS=’ NEW ’ ) 

C RADIAN/DEGREE CONVERSION FACTORS 
RTOD=S7. 29S779S1 
DT0R=0. 0174S293 



6 - 



non o noooc: 



C COMPUTE 4X4 IDENTITY MATRIX 
DO 5 1=1,2 
DO 5 J=l,2 
IF (I.EQ. J) THEN 

IMAT(I,J)=1. 0 

ELSE 



IMAT(I,J)=0. 0 

ENDIF 

5 CONTINUE 



H(l,2)=0. 0 



C INITIALIZE TIME COUNTER 
T0TTIM=0. 0 
T I MEM 1=0. 0 
\vIND=0. 0 
NP=0 



C INITIALIZE COUNTER FOR MANEUVER GATE 
E1M1=0. 0 
E1M2=0. 0 



' COMPUTE BEARING MEASUREMENT COVARIANCE 

BEARING ERROR STANDARD DEVIATION = 1 NM 

WRITE(*,*) 'FILTERING OBSERVED DATA WITH KALMAN FILTER' 

WR I TE ( * , ) ' -v.v-.v=====*-.v* ' 

1 0 READ( 2,1001, END=800') DATE , HR , MN , LAT , A , LONG , B , PCN , WINDD , NAV , NET 

RADAR DATA FOR MEASUREMENT NOISE COV. MATRIX 
1F(PCN. EQ. DTHEN 
AS=100. 0 



ELSEIF(PCN. EQ, 2)THEN 
AS=225. 0 

ELSE IF( PCN. EQ. 3)TI1EN 
AS=623. 0 

ELSEIF(PCN. EQ. 4)THEN 
AS=900. 0 



AIRCRAFT DATA 



ELSE 

AS=( (NAV)**2+(MET)**2)**0. 5 
ENDIF 
R(1,1)=AS 
R(l,2)=0. 0 
R(2,l)=0. 0 
R(2,2)=AS 



V? V? Vr Vr Vr Vr Vr V^**W^**V V?%Wr Vr Vr Vr Vr %V V? %W% Vr Vr Vf Vf Vr Vf V'? V'T Vr Vr Vr Vr Vr 



READ IN OBSERVATION PACKET ( DATE , TINE , LAT, LONG) 
DT=TIME(K)-TIME(K-1) 
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F0RMAT(F6. 0,F2. 0 ,F2. 0 ,F3. 0 , Al ,F4. 0 , A1 , 1 1 ,F3. 0 , 2( F2. 0)) 



MN=MN/60. 0 
LAT=LAT/10 
LONG=LONG/ 10 
TIME=HR+MN 

1 F0RMAT(1X,F7. 0,4X,F3. 0, 1X,F6. 4,6X,F4. 1,A1,3X,F5. 1,A1) 
NP=NP+1 

IF (NP.EQ. 1) THEN 
DATE1=DATE 
TIMEMi=TIME 
END IF 

IF (DATE. NE. DATE 1) THEN 
TIME=TIME+24 
DT=TIME-TIMEM1 
TIME=TIME-24 
ELSE 

DT=TIME-TIMEM1 
END IF 

DTF=DT*60. 0 
DTS(NP)=DT 
TOTTIM=TOTTII!4-DT 
C WRITE (■••',*) DT,NP,WINDD 

2 F0RMAT(1X,F7. 4,5X,F6. 2,5X,F6. 2) 

CALL FINDPKI(PHI,DT) 

Z( 1 , 1)=WINDD 
ZY=WINDD 

IFCNP.EQ. 1) THEN 

CALL INIT(WINDD,XKK,PKK) 

C WRITE(--S*)'X(0|0,0): ’ 

DO 601 1=1,2 
LXKK(I,1)=XKK(I,1) 

C WRITS ( 3 , ■• ) ’ •-'“•>**-,v*^>****** ' 

C WRITEC*,*) (XKK( 1,1) ,1=1,2) 

601 CONTINUE 

WRITE(* ,-'0 ' **********->** ' 

WRITE (■>,*) ZY 



C WRITE(3,*) ’P(0|0,0); ' 

DO 602 1=1,2 
DO 602 J=l,2 

C LPKK(I,J)=PKK(I,J) 

C WRITE(3.401)PKK(I,J) 

401 F0RMAT(2F14. 4) 

602 CONTINUE 

END IF 



C PROJECT AHEAD STATE AND ERROR COVARIANCE ESTIMATES 
C X(K+1|K) = PHI * X(K|K) 

CALL MATMULC PHI , XKK ,2,2,1, XKKM 1 ) 
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DO 603 1=1,2 
LXKKM1(I,1)=XKKM1(I,1) 
603 CONTINUE 



C P(K+1|K) = (PHI * P(K(K) * PHIT) + Q 

CALL MATRAN(PKI,PHIT,2,2) 

CALL MATMULC PHI , PKK , 2 , 2 . 2 , TEMP6 ) 

CALL MATMULC TEMP6 , PHIT ,2,2,2, TEMP7 ) 
CALL GETQ(Q,DT) 

CALL MATADDC TEMP 7 , Q , 2 , 2 , 1 , PKKMl ) 

DO 408 1=1,2 
DO 408 J=l,2 

LPKKM1(I,J)=PKKM1(I,J) 

408 CONTINUE 



204 CONTINUE 



COMPUTE OBSERVATION RESIDUAL 
E=Z(K)-H*X(K[K-1) 

IFCWINDD. EQ. 0)THEN 
E(1,1)=0. 0 
E(2,l)=0. 0 
ELSE 

CALL MATMULC H , XKKM 1 , 2 , 2 , 1 , TEMP 1 ) 

CALL MATSUB(Z,TEMP1,2,1,E) 

END IF 

COMPUTE VARIANCE OF RESIDUALS SEQUENCE 
AND ADAPTIVE GATE VALUE 
VAR( E ) =H--'-PKKM l*HT-i-R 

CALL MATRANCH,HT,2,2) 

CALL MATMULC H , PKKM 1 , 2 , 2 , 2 . TEMP2 ) 

CALL MATMULC TEMP2 , HT . 2 , 2 , 2 , TEMP3 ) 

CALL MATADDC TEMPS , R , 2 , 2 , 1 , VARE ) 

WRITEC 3, •••') 'VARIANCE OF RESIDUALS = ' ,VARE 
GATE 1=1. 5->SQRTCVARE) 



COMPUTE KALMAN GAIN MATRIX 

G=PKKM1'->HP--C H-'--PKKMl--^HT+R)**- 1 
CALL MATRAN(H,HT,2,2) 

CALL MAT::ULC PKKM l , HT , 2 , 2 , 2 , TEMP4 ) 
CALL MATINV(VARE,2,IVARE) 

CALL MATMULC TEMP4 , 1 VARE , 2 , 2 , 1 , G) 



COMPUTE UPDATED ESTIMATE 

XCKiK)=X(K|K-l)+G-"-E, WHERE E=ZCK)-H*X(K[K-1) 
CALL MATMULC G . E . 2 , 2 , 1 ,TEHP5 ) 

CALL MATADDCTEMPS ,XKKM1 ,2,1,1 ,XKK) 
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c 



WRITE(3,*)'X( ' ,TIME, ' | ’ ,TIME, ' , ' ,L, ' ' 
DO 605 1=1,2 
C WRITE(3,*)XKK(I,1) 

605 CONTINUE 



C COMPUTE UPDATED ERROR COVARIANCE MATRIX 
C P(K|K)=(I - G*H)*P(K|K-1) 

CALL MATMUL(G,H,2,2,2,TEMP6) 

CALL MATSUB ( IMAT , TEMP6 ,2,2, TEMP 7 ) 
CALL MATMUL(TEMP7,PKKM1,2,2,2,PKK) 



C THESE STATEMENTS ARE FOR THE SMOOTHING ALGORITHM 

DO 620 1=1,2 
XKKS(I,1,NP)=XKK(I,1) 

C WRI TE ( , * ) XKKS ( 1 , 1 , NP ) , PKKS ( 1 , 1 , NP ) 

620 CONTINUE 

DO 630 1=1,2 
DO 630 J=l,2 

PKKS(I,J,NP)=PKK(I,J) 

630 CONTINUE 

VRITE(3,*) 'FILTERED DATA FOR DATA POINT' ,NP 
WRITE(3,*) 'TIME VEL. ACCELL. HEADING SPEED' 
VRITE( 3 , * )TOTTIM , XKK( 1 , 1 ) , XKK( 2 , 1 ) 
WRITE(4,*)TOTTIM,ZY 

WRITE ( 5 , ) TOTTI M , XKK( 1 , 1 ) , XKK( 2 , 1 ) , PKK( 1 , 1 ) 
WRITE(9,-'-)NP 



1002 FORMAT( 1X,5F10. 3) 

1003 FORMATC IX, F6. 2,3X,F10. 1,2X,F11. 1,3X,F8. 1,3X,F8. 1) 

1004 FORMAT( 1X,F6. 2,3(F8. 1,2X)) 

C COMPARE BEARING ERRORS TO MANEUVER DETECTION GATES 

IF ((A3S(M1). GT. (GATEl))) THEN 
C WRITE( '■%•>) '**-'• MANEUVER DETECTION ***' 

C V;R I TE( 3, -■■-)'**••> MANEUVER DETECTION 

CALL RE I N IT( DT , Z Y , Z YM 1 , LPKKM 1 , XKKM 1 , PKKM 1 ) 
E1M1=0. 0 
E1M2=0. 0 
GOTO 204 

ENDIF 



TIMEM1=TIME 

DATE1=DATE 

ZYM1=ZY 
GOTO 810 

WRITE C 6 , ’’0 TOTTIM , XKK( 1,1), XKK( 2,1), PKK( 1,1) 

C THIS IS WHERE THE SMOOTHING ALGORITHM STARTS 
C FIXED INTERVAL SMOOTlilNG 

800 WRITE('-'-,-'0 'SMOOTHING FILTERED DATA WITH A' 

WRITE(*,-'-) 'FIXED INTERVAL SMOOTHING ALGORITHM’ 



novo on non 



VR I TE ( * , * ) ' ***====>wc* ' 

C WRITE (*,*) DT,NP,WINDD 

DO 1000 KK=1,KP-1 

C CALL REINIT(DT,ZY,ZYM1,LPKKM1,XKKM1,PKKM1) 

K=NP-KK 
DT=DTS(K+1) 

TIME=TIMEM1-DT 
TOTTIM=TOTTIM-DT 
CALL FINDPHI(PHI,DT) 

DO 901 1=1,2 
XSS(I,1)=XKKS(I,1,K) 

901 CONTINUE 

DO 902 1=1,2 
DO 902 J=l,2 
PSS(I,J)=PKKS(I,J,K) 

902 CONTINUE 



CALCULATE THE PREDICTED STATE AND ERROR COVARIANCE MATRICES 
X(K+1|K)=PHI*X(K|K) 

CALL MATMUL ( PHI ,XSS , 2 , 2 , 1 ,XKKM1S) 

P(K+1 I K)=PHI*P( K I K)*PHIT+Q 

CALL MATRAN (PHI ,PHIT,2,2) 

CALL MATMULC PHI , PSS , 2 , 2 , 2 .TEMP6) 

CALL MATMULC TEMP6 , PHIT , 2 , 2 , 2 , TEMP7 ) 

CALL GETQ(Q,DT) 

CALL MATADD ( TEMP 7 , Q , 2 , 2 , 1 , PKKM 1 S ) 



CALCULATE THE SMOOTHING FILTER GAIN MATRIX 
AK=P ( K I K ) ’-'PH I T--- INV “ P ( K+ 1 1 K ) 

CALL MATINV ( PKKMIS , 2 , IPKKMIS) 

CALL MATMUL (PKKMIS , IPKKMIS , 2 , 2 , 2 , II) 
CALL MATMUL ( PSS , PHIT, 2 , 2 , 2 ,TEMP1S) 
CALL MATMUL (TEMPIS , IPKKMIS , 2 , 2 ,2 , AK) 

DO 904 1=1,2 

XNNMK 1 , 1)=XKKS( 1 , 1 ,K+1) 

'4 CONTINUE 



CALCULATE THE SMOOTHED STATE ESTIMATE 
XKKS=X ( K I K ) + AK* ( X ( K+ 1 1 N ) - X ( K+ 1 1 K ) 

CALL MATSUB (XNNMl ,XKKM1S ,2,1 ,TEMP2S) 
CALL MATMUL ( AK ,TEMP2S , 2 , 2 , 1 ,TEMP3S) 
CALL MATADD (XSS ,TEMP3S , 2 , 1 ,K ,XKKS) 

DO 906 1=1,2 
DO 906 J=l,2 

PNNM 1(I,J)=PKKS(1,J,K+1) 

906 CONTINUE 

C CALCULATE THE SMOOTHED COVARIANCE MATRIX 
C FKKS=P(K|K)+AK*[ P(K+1 | N) -P(K+1 | K)] *AKT 
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CALL MATSUB (PNNMl , PKKMIS , 2 , 2 ,TEMP4S) 

CALL MATRAN (AK,AKT,2,2) 

CALL MATMUL ( AK,TEMP4S , 2 , 2 , 2 .TEMP5S) 

CALL MATMUL (TEMP5S ,AKT,2,2 ,2,TEMP6S) 

CALL MATADD (PSS,TEMP6S,2,2,K,PKKS) 

WRITE(3,*) 'SMOOTHED DATA FOR DATA POINT' ,K 
WRITE(3,*) 'TIME VEL. ACCEL. HEADING SPEED' 

WR I TE ( 3 , * ) TOTTI M , XKKS ( 1 , 1 , K ) , XKKS ( 2 , 1 , K ) 
WRITE(6,*)TOTTIM,XKKS(l,l,K),XKKS(2,l,K),PKKS(l,l,K) 
1010 FORMATC 1X,5F10. 3) 

1020 FORMATC IX, F6. 2, 3X,F10. 1,2X,F11. 1,3X,F8. 1,3X,F8. 1) 

1030 FORMATC IX, F6. 2, 3(F8. 1,2X)) 

TIMEM1=TIME 
1000 CO.NTIKUE 



1100 CONTINUE 

1110 FORMATC 14, 2F8. 1) 

1120 FORMATC 14, 3C F8. 1,2X)) 

CLOSE C UN IT=2) 

CL0SECUNIT=3) 

CLOSE C UN IT=4) 

CLOSECUNIT=5) 

CLOSECUNIT=6) 

CLOSECUNIT=9) 

CLOSECUNIT=S) 

WRITEC-S*) 'FILTERED & SMOOTHED OUTPUT DATA IS LOCATED IN THE' 
WRITEC-S*) 'DATA FILE OUTDATA.DAT. FOR GRAPHIC RESULTS,' 
WRITEC",'-) 'ENSURE OBSDATA.DAT, FILDATA.DAT, & SMDATA.DAT ARE' 
WRITEC*,*) 'IN THE MATLAB SUB-DIRECTORY AND RUN THE MATLAB' 
WRITEC •S") 'M-FILE STORM2.M' 

STOP 

END 



C SUBROUTINES 



SUBROUTINE FI.NDPHICPHI ,DT) 

Q Vc Vr •;> Vc Vr -.V Vc iV Vr Vr ‘V Vr Vr ‘V Vr iV Vr V.- ; V Vc *.V ‘V Vr Vr Vr Vr Vr Vr Vf *.V •>> -sV Vr •;> Vr Vr Vr V? * Vr * V? * * VcVr * /VVr * Vr Vr ‘V 

C COMPUTES THE VALUES OF THE PHI MATRIX 

REAL^'-4 PHIC2,2),DT 

C DO 1501 1=3,4 

C DO 1501 J=l,4 

PHICI,J)=0. 0 

C501 CONTINUE 

C COMPUTE PHI MATRIX 
DO 1500 1=1,2 
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PHI(I,I)=1. 0 
1500 CONTINUE 

PHI(1,2)=DT 
PHI(2,1)=0. 0 
PHI(2,3)=0. 0 
PHI(2,4)=0. 0 
PHI(1,3)=0. 0 
PHI(1,4)=0. 0 

RETURN 



END 



SUBROUTINE INIT(WINDD,XKK,PKK) 

THIS ROUTINE INITIALIZES THE STATE 
AND ERROR COVARIANCE ESTIMATES 

Vf V f *: V V r Vr Vr * V Vr ‘A* Vr V: "iV -V -V * V Vr -V * V - V Vr iV Vr V -A- Vr *,V : V Vr 'V *V V? Vr Vr * Vc Vr Vr Vr A* Vr Vr * 

REAL*4 XKK(1,1),PKK(2,2) 

REAL*4 WIND,V.’INDD 

INITIAL STATE ESTIMATE 
XKK(1,1)=WINDD 
WRITE(*,*) XKK(1,1) 

XKK(3, 1)=0. 0 
XKK(4, 1)=0. 0 

INITIAL ERROR COVARIANCE ESTIMATE 
PKK( 1, 1)=1000000. 

PKK(1,2)=0. 0 
PKK(1,3)=0. 0 
PKK(1,4)=0. 0 
PKK(2,1)=0. 0 
PKK(2,2)=0. 25 
PKK(2,3)=0. 0 
PKK(2,4)=0. 0 
PKK(3,1)=0. 0 
PKK(3,2)=0. 0 
PKK(3,3)=0. 0 
FKK(3,4)=0. 0 
PKK(4,1)=0. 0 
PKK(4,2)=0. 0 
PKK(4,3)=0. 0 
PKK(4,4)=0. 0 

RETURN 



END 

SUBROUTINE GETQ(Q,DT) 






ROUTINE TO GET Q MATRIX 

;> Vc ■: VVr “Wr */r -,V A- V- A* Vr *.’r 'V A- * V Vr *V V.- *V -.V V? •/? "'r A- Vr Vr Vf V: Vr V? Vr Vc Vr *,V Vf Vr V? V; Vr Vr Vr Vr V- Vr Vr V- -V* V Vr 'V • V '.V Vr -V % V 

REAL’’-4 Q(2,2),DT 
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C DO 100 1=1,4 

C DO 100 J=3,4 

COO Q(I,J)=0. 0 

Q(l,l)=(DT-*4)/4. 

Q(l,2)=(DT**3)/2. 

Q(2,l)=(DT’>*3)/2. 

Q(2,2)=(DT’’"'^2) 

C DO 200 1=3,4 

C DO 200 J=l,4 

COO Q(I,J)=0.0 

RETURN 



END 



SUBROUTINE RE INIT( DT , ZY , ZYMl , LPKKMl , XKKMl , PKKMl ) 

C THIS ROUTINE RE-INITIALIZES THE STATE AND ERROR 

C COVARIANCE ESTIMATES 

REAL*4 DT , XKKn 1(2,1). PKKM 1(2,2) 

RE AL^'- 4 ZX , Z Y , ZXM 1 , ZYM 1 , LPKKM 1(2,2) 

C XDIFF=ZX-ZXM1 

C YDIFF=ZY-ZYM1 



C 

C 

C 



100 



c 

c 



c 

c 

c 

c 

c 

c 

c 

c 

c 

c 



XKKM1(1,1)=ZX 
XKKM1(1,1)=ZY 
XKKM1(3,1)=0. 0 
XKKM1(4, 1)=0. 0 

WRITE(*,*) 'REINITIALIZED STATES ARE:' 
DO 100 1=1,2 

WRITE(*,*)XKKM1(I,1) 

CONTINUE 

PKKM1( 1 , 1)=2. 25*LPKKM1( 1,1) 

PKKM1(1,2)=0. 0 

PKKM 1 ( 1 , 3 ) =2 . 25*LPKKM 1( 1,3) 

PKKM1(1,4)=0. 0 

PKKM1(2,1)=0. 0 

PKKM1(2,2)=0. 1111 

PKKM1(2,3)=0. 0 

PKKM1(2,4)=0. 0 

PKKMK 3 , 1 )=2. 25 ''-LPKKM 1( 3,1) 

PKKM1(3,2)=0. 0 

PKKM 1 ( 3 , 3 ) =2 . 25*LPKKM1 ( 3,3) 

PKKM1(3,4)=0. 0 

PKKM1(4,1)=0. 0 

PKKM1(4,2)=0. 0 

PKKM1(4,3)=0. 0 

PKKM1(4,4)=0. 1111 



RETURN 



ooooo o oooo 



END 



SUBROUTINE MP( XS 1 , YS 1 , XS2 , YS2 , BRG 1 , BRG2 , ZX , ZY ) 

•>V*Vc*Vr*Vf*****VrVf*:V**VfVr:V‘>V*V»*y»******:V**VfVrVr*****'jV*>V'}V**';V*Vc* 

THIS ROUTINE COMPUTES THE ESTIMATED 
X,Y POSITION OBTAINED FROM MEASUREMENTS 

REAL*4 ZX,ZY 

RE AL*4 XS 1 , YS 1 , XS 2 , YS2 , BRG 1 , BRG2 
REAL’H NUMER,DENOM 

INITIAL STATE ESTIMATE 

NUMER=( - YS 2*TAN ( BRG2 ) ) +( YS 1*TAN( BRG 1 ) ) +XS2 -XS 1 
DENOM=TAN( BRGl) -TAN( BRG2) 

ZY=NUMER/DEKOM 

ZX=( ZY - Y S 1 ) *TAN( BRG 1 ) +XS 1 



RETURN 



END 



SUBROUTINE ELLIP(XT,YT,P1 ,P3 ,P13) 

*/V Vc ‘A' Vc “V ‘V Vr ‘V V? A- “V V? Vr -A -A- -A- A* A A' A- A A A A A A A- A' A A A* A A- A' A- A' A- A A' A' A- A' A- A' Vr A' A A A A' 

THIS SUBROUTINE COMPUTES ERROR ELLIPSE DATA 
FROM ERROR COVARIANCE DATA 

DIMENSIONS AND DECLARATIONS 

REAL*4 XT,YT,XP(21) ,YP(21) ,A,B ,THE1 ,SIG2X,SIG2Y 
RE AL-'-4 SX , S Y , PT , CT , ST , P 1 , P 1 3 , P3 



A=2---P13 

B=P1-P3 

THE 1=0. 5*ATAN2(A,B) 

A=(Pl+P3)/2 
B=0. 0 

IF (P13.EO. 0.0) GOTO 10 
B=P13/SIN(2. 0-'--THEl) 

10 SIG2X=ABS(A+B) 

SIG2Y=ABS(A-E) 

SX=SIG2X^’--->0. 5 
SY=SIG2Y**0. 5 
PT=3. 141592654/10 
CT=C0S(THE1) 

ST=SIN(THE1) 

DO 100 IE=1,21 

XP ( I E ) =SX '•-C OS ( PT* IE ) *CT - S Y* S I N ( PT* I E ) *ST+.XT 
YP( IE)=S.X*COS( PT*IE)*ST+SY*SIN( PT*IE)*CT+YT 
VRITEC 7 , *) XP( IE ) , CHAR( 9 ) , YP( IE ) 

100 CONTINUE 



RETURN 



ooooo 1-* h-* oooon 



END 



SUBROUTINE KATMUL(A,B,L,M,N,C) 



***** Vc VwV^V Vc- VryrVrVr-jVVc-'jV* 



THIS ROUTINE MULTIPLIES TVv’O MATRICES TOGETHER 
° C(L,N) = A(L,M) * B(M,N) 

DIMENSIONS AND DECLARATIONS 
REAL*4 A(L,M),B(M,N),C(L,N) 



DO 10 1=1, L 
DO 10 J=1,N 
C(I,J)=0. 0 
CONTINUE 



DO 100 1= 
DO 100 J= 
DO 100 K= 
C(I,J) = 
CONTINUE 



1,L 

l.N 

1,M 

C(I,J) + A(I,K)*B(K,J) 



RETURN 



END 



SUBROUTINE MATRAN(A,B,N,M) 

* * * * * ** ** * * **** Vf ** * * * * * * * * Vr * * * ■;>* * * Vr* * * * * 

THIS ROUTINE TRANSPOSES A MATRIX 
“ B(M,N) = A'(N,M) 

V: * * * * * * Vr * * * * * * * * * * * * * V r * V r * * * * * * * * * * * * * * * * 

DIMENSIONS AND DECLARATIONS 
REAL*4 A(N,M), B(M,N) 

DO 100 1= l.N 
DO 100 J= 1,M 
B(J.I) = A(I,J) 

100 CONTINUE 

RETURN 

END 



SUBROUTINE MATSCL( Q , A , N , M , C ) 

Q * * * * * * * Vr * * * * *' * * * * * * * * •. V * ^ f * ‘.V* * * * * * * * * * * * * * * * * * * * * Vr * ** * 

C THIS ROUTINE MULTIPLIES A MATRIX WITH A SCALAR 

C “ C(N,H) = Q * A(N,M) 

V»-******************';V****Vr*^V******************-Vc’*-****** 

DIMENSIONS AND DECLARATIONS 

REAL^--4 A(N,M), C(N,M), Q 



DO 100 I = l.N 
DO 100 J = 1,M 
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C(I,J) = Q*A(I,J) 
100 CONTINUE 

RETURN 

END 



SUBROUTINE MATSUB( A,B ,N,M,C) 

C THIS ROUTINE SUBTRACTS T\^Q MATRICES 

C “ C(N,M) = A(N,M) - B(N,M) 

C DIMENSIONS AND DECLARATIONS 

RE A L*4 A ( N , M ) , B ( N , M ) , C ( N , M ) 

DO 100 I = 1,N 

DO 100 J = 1,M 

C(I,J)=A(I,J)-B(I,J) 

100 CONTINUE 

RETURN 

END 

SUBROUTINE MATADD( A , B , N , M , L , C ) 

Q Vr V? Vr Vr Vr Vr : V :V V? 'j'c Vr Vr Vr V: Vr Vr Vr “V Vr Vr V? Vr Vr Vr Vr Vr Vr V,' Vr “V Vr * V? Vr Vr Vr Vf Vr Vr Vr Vc Vr Vr 

C THIS ROUTINE ADDS TYO MATRICES 

C ° C(N,M) = A(N,M) + B(N,M) 

Q Vr Vf Vr “V Vc Vr Vr Vr “V Vr V? ■:> V,' Vr “V Vr Vr * V *.V */r : V ‘V “V •;? Vr Vr Vr Vj* V? Vc Vc* Vr V • ‘.V :V Vr Vr V? Vc Vr V : “V *V 

C DIMENSIONS AND DECLARATIONS 

REAL*4 A(N,M),B(N,M) ,C(N,M,L) 

DO 100 I = 1,N 

DO 100 J = 1,M 

C(I,J.L)=A(I,J)+B(I,J) 

100 CONTINUE 

RETURN 

END 



SUBROUTINE MATINV (A,N,C) 

QVcV?VrVrVr:VVr:VVrV?VrV?VrVc:VVrVrVr:VVrVcVrV?V:';V*VrVr'.VVrVrVrVfVrVfVr*V?***'5V*VrVr 

C THIS ROUTINE COMPUTES THE INVERSE OF 

C A MATRIX 

C C(N,N)=INV [A(N,N)] 

Q Vr Vr Vr “V Vr ;V *, V ■>; Vc V% Vr :V ‘V x‘: “V “V ;V Vr Vc ‘A* ?V Vr : V V */: *V “V ;V “’c Vr Vr Vc Vc* Vc V? V? ‘V Vr :V V r V? Vr Vc 

C DIMENSIONS AND DECLARATIONS 

R E A L--'^ 4 A ( N . N ) . C ( N , N ) , D ( 2 0 , 2 0 ) 

DO 100 I = 1,N 
DO 100 J = 1,N 
100 D(I.J)=A(I,J) 

DO 115 1=1, N 
DO 115 J=N+1.2-'-N 
115 D(I,J)=0.0 



120 



140 

160 

180 

185 

200 

220 

240 

260 

265 



DO 120 1=1, N 
J=I+N 

D(I,J) = 1. 0 

DO 240 K=1,N 
M=K+1 

IF (K. EQ. N) GOTO 180 
L— K 

DO 140 I=M,N 

IF (ABS(D(I,K)). GT. ABS(D(L,K))) L=I 
IF (L. EQ. K) GOTO 180 

DO 160 J=K,2*N 
TEMP=D(K,J) 

D(K,J)=D(L,J) 

D(L,J)=TEMP 

DO 185 J=M,2*N 
D(K,J)=D(K,J)/D(K,K) 

IF (K. EQ. 1) GOTO 220 
M1=K-1 

DO 200 1=1, Ml 
DO 200 J=M,2*N 
D(I,J)=D(I,J)-D(I,K)*D(K,J) 

IF (K. EQ. K) GOTO 260 

DO 240 I=M,N 
DO 240 J=M,2*N 

D(I,J)=D(I,J)-D(I,K)*D(K,J) 

DO 265 1=1, N 
DO 265 J=1,N 
K=J+N 

C(I,J)=D(I,K) 

RETURN 

END 
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